基于 CasADi 的三阶段地形跟随轨迹优化与 Bang-Bang 控制（含完整数学建模）

代码对应文件：terrain_following_optimizer/src/optimizer/trajectory_optimizer_casadi.py

运行脚本入口：同名 Python 文件 __main__ 部分

---

1. 真实任务背景与非线性规划建模总览

在很多典型的对地攻击 / 侦察任务中，飞机需要在给定起终点与终端速度约束 下，尽可能贴地飞行，以减小暴露时间，同时避免撞地并满足飞行包线约束。

本文讨论的就是这样一个地形跟随最优控制问题 ：在满足终端高度、终端速度、攻角和油门约束的前提下，寻找一条最优的三维轨迹和对应的油门历史，使得飞行时间与地形跟踪误差的加权和最小。

从实现角度看，我们做了两件事：

• 先在物理空间写出常规的动力学方程和任务约束；
• 再把问题转化为以航程为自变量的非线性规划（NLP），并进行无量纲化，便于数值优化。

下面先给出飞机与任务的基本参数，然后再进入状态方程和 NLP 建模。

1.0 飞机与飞行任务基本参数

本文所用的飞机模型及飞行任务参数如下表所示：

飞机物理参数：

参数	符号	数值	单位	说明
重量	WWW	34000	lb	W=mgW = mgW=mg，质量约 1056 slug
参考面积	SSS	500	ft²	机翼参考面积
海平面音速	vsv_svs	1116.4	ft/s	标准大气
重力加速度	ggg	32.174	ft/s²	---
海平面密度	ρ0\rho_0ρ0	0.002377	slug/ft³	标准大气
大气标高	HscaleH_{\text{scale}}Hscale	23800	ft	指数大气模型

飞行约束：

参数	符号	最小值	最大值	单位
攻角	α\alphaα	-10° (-0.175 rad)	20° (0.349 rad)	deg/rad
油门	η\etaη	0.0	1.0	---
速度	VVV	100	3000	ft/s

边界条件（典型任务）：

参数	初始值 (X0X_0X0)	终端值 (XfX_fXf)	单位
航程 XXX	0	64000	ft
高度 hhh	F(0)F(0)F(0) (地形)	F(64000)F(64000)F(64000) (地形)	ft
速度 VVV	557.5 (M ≈ 0.5)	1114.9 (M ≈ 1.0)	ft/s
飞行路径角 γ\gammaγ	0	0	rad

基本力学公式：

动压：
q=12ρV2 q = \frac{1}{2} \rho V^2 q=21ρV2

升力（线性模型 CL=CLα⋅αC_L = C_{L\alpha} \cdot \alphaCL=CLα⋅α）：
L=q⋅S⋅CL=q⋅S⋅CLα⋅α L = q \cdot S \cdot C_L = q \cdot S \cdot C_{L\alpha} \cdot \alpha L=q⋅S⋅CL=q⋅S⋅CLα⋅α

阻力：
D=q⋅S⋅CD D = q \cdot S \cdot C_D D=q⋅S⋅CD

推力（油门控制）：
T=η⋅Tmax⁡(M,h) T = \eta \cdot T_{\max}(M, h) T=η⋅Tmax(M,h)

其中 Tmax⁡(M,h)T_{\max}(M, h)Tmax(M,h) 由推力表双线性插值得到。

最大推力表 Tmax⁡(M,h)T_{\max}(M, h)Tmax(M,h)（单位：千磅 klb）：

M \ h (kft)	0	5	10	15	20	25	30	40	50	70
0.0	24.2	---	---	---	---	---	---	---	---	---
0.2	28.0	24.6	21.1	18.1	15.2	12.8	10.7	---	---	---
0.4	28.3	25.2	21.9	18.7	15.9	13.4	11.2	7.3	4.4	---
0.6	30.8	27.2	23.8	20.5	17.3	14.7	12.3	8.1	4.9	---
0.8	33.5	30.3	26.6	23.2	19.8	16.8	14.1	9.4	5.6	1.1
1.0	37.9	34.2	30.4	26.8	23.3	19.8	16.8	11.2	6.8	1.4
1.2	36.1	38.0	34.9	31.3	27.3	23.6	20.1	13.4	8.3	1.7
1.4	---	36.6	38.5	36.1	31.6	28.1	24.2	16.2	10.0	2.2
1.6	---	---	---	38.7	35.7	32.0	28.1	19.3	11.9	2.9
1.8	---	---	---	---	---	34.6	31.1	21.7	13.3	3.1

说明：

• 行表示马赫数 MMM，列表示高度 hhh（千英尺 kft）
• "---" 表示该飞行状态超出发动机工作包络（数据不可用）
• 实际推力 T=η⋅Tmax⁡×1000T = \eta \cdot T_{\max} \times 1000T=η⋅Tmax×1000 lb，其中 η∈0,1\eta \in 0, 1η∈0,1 是油门开度
• 使用双线性插值 获取任意 (M,h)(M, h)(M,h) 点的推力值

质点动力学方程（纵向平面）：

沿轨迹切向（速度方向）：
mdVdt=Tcos⁡α−D−Wsin⁡γ m \frac{dV}{dt} = T \cos\alpha - D - W \sin\gamma mdtdV=Tcosα−D−Wsinγ

沿轨迹法向：
mVdγdt=Tsin⁡α+L−Wcos⁡γ m V \frac{d\gamma}{dt} = T \sin\alpha + L - W \cos\gamma mVdtdγ=Tsinα+L−Wcosγ

运动学关系：
dXdt=Vcos⁡γ,dhdt=Vsin⁡γ \frac{dX}{dt} = V \cos\gamma, \quad \frac{dh}{dt} = V \sin\gamma dtdX=Vcosγ,dtdh=Vsinγ

气动系数模型（多项式拟合）：

升力系数斜率：
CLα(M)=2.2371+0.3400 M−0.2615 M2+0.01085 M3 C_{L\alpha}(M) = 2.2371 + 0.3400\,M - 0.2615\,M^2 + 0.01085\,M^3 CLα(M)=2.2371+0.3400M−0.2615M2+0.01085M3

零升阻力系数：
CD0(M)=0.0065−0.002218 M−0.01649 M2+0.04384 M3−0.02840 M4+0.004593 M5 C_{D0}(M) = 0.0065 - 0.002218\,M - 0.01649\,M^2 + 0.04384\,M^3 - 0.02840\,M^4 + 0.004593\,M^5 CD0(M)=0.0065−0.002218M−0.01649M2+0.04384M3−0.02840M4+0.004593M5

总阻力系数（抛物线极曲线）：
CD=CD0+CLα⋅α2 C_D = C_{D0} + C_{L\alpha} \cdot \alpha^2 CD=CD0+CLα⋅α2

大气模型（指数衰减）：

ρ(h)=ρ0⋅exp⁡(−hHscale) \rho(h) = \rho_0 \cdot \exp\left(-\frac{h}{H_{\text{scale}}}\right) ρ(h)=ρ0⋅exp(−Hscaleh)

地形约束：

飞行高度必须始终高于地形：
h(X)≥F(X)+hclearance h(X) \ge F(X) + h_{\text{clearance}} h(X)≥F(X)+hclearance

其中 F(X)F(X)F(X) 是地形高程函数，hclearanceh_{\text{clearance}}hclearance 是安全裕度（本文取 50 ft）。

---

1.1 状态变量与独立变量

原始的飞行动力学通常以时间 ttt为自变量，状态为
(X(t), h(t), V(t), γ(t), α(t)) (X(t),\, h(t),\, V(t),\, \gamma(t),\, \alpha(t)) (X(t),h(t),V(t),γ(t),α(t))

其中

• XXX：沿地面投影的航程（ft）
• hhh：高度（ft）
• VVV：速度（ft/s）
• γ\gammaγ：飞行路径角（相对水平线）
• α\alphaα：攻角

为了做地形跟随和终端约束，本文采用以航程 (X) 为自变量 的形式：
x=X∈X0,Xf x = X \in X_0, X_f x=X∈X0,Xf

这时高度直接由轨迹函数h(X)h(X)h(X)给出，而不是作为微分方程中的独立状态。这一点在代码中体现在：高度由样条插值 spline_interp.evaluate(h_nodes, X) 给出，而不是积分得到。

真正需要积分的状态主要是：

• 速度V(X)V(X)V(X)
• "地形约束松弛量"ζ(X)\zeta(X)ζ(X)（用于累计是否穿地）

代码中的积分循环（for step in range(self.num_steps)）就是在均匀的网格
Xk=X0+k ΔX,k=0,...,N−1 X_k = X_0 + k\,\Delta X,\quad k=0,\dots,N-1 Xk=X0+kΔX,k=0,...,N−1

上，用欧拉法近似求解
dVdX=fV(X,V,h,γ,α,η),dζdX=fζ(X,h) \frac{dV}{dX} = f_V(X, V, h, \gamma, \alpha, \eta),\qquad \frac{d\zeta}{dX} = f_\zeta(X, h) dXdV=fV(X,V,h,γ,α,η),dXdζ=fζ(X,h)

1.2 目标：时间与地形跟踪的加权

优化目标函数在代码中构造为
J=ϕ Tflight+(1−ϕ) Jtrack,norm J = \phi\, T_{\text{flight}} + (1-\phi)\,J_{\text{track,norm}} J=ϕTflight+(1−ϕ)Jtrack,norm

其中

• 飞行时间积分：
  Tflight=∫X0Xf1V(X)cos⁡γ(X) dX≈∑kΔXVkcos⁡γk T_{\text{flight}} = \int_{X_0}^{X_f} \frac{1}{V(X)\cos\gamma(X)} \, dX \approx \sum_k \frac{\Delta X}{V_k \cos\gamma_k} Tflight=∫X0XfV(X)cosγ(X)1dX≈k∑VkcosγkΔX

  对应代码：

  dt = self.dX / (V * ca.cos(gamma) + 1e-6)
  flight_time_integral += dt

• 地形跟踪误差（未归一化形式）：
  Jtrack=∫X0Xf(h(X)−F(X))2 dX≈∑k(hk−Fk)2ΔX J_{\text{track}} = \int_{X_0}^{X_f} (h(X) - F(X))^2\, dX \approx \sum_k (h_k - F_k)^2 \Delta X Jtrack=∫X0Xf(h(X)−F(X))2dX≈k∑(hk−Fk)2ΔX

  对应代码：

  tracking_error_integral += (h_curr - F_terrain)**2 * self.dX

• 为了避免数量级差异过大，引入归一化：
  Jtrack,norm=Jtrack(Xf−X0) href2 J_{\text{track,norm}} = \frac{J_{\text{track}}}{(X_f-X_0)\, h_{\text{ref}}^2} Jtrack,norm=(Xf−X0)href2Jtrack

  对应代码：

  total_distance = self.bc.X_f - self.bc.X_0
  h_ref = 100.0
  tracking_error_normalized = tracking_error_integral / total_distance / (h_ref**2)

1.3 无量纲化：论文的归一化体系

为了使方程更加紧凑、数值更加稳定，论文采用了如下无量纲化：

无量纲状态变量（论文公式 7）：
H=g yvs2,Ξ=g xvs2,M=vvs,τ=g tvs H = \frac{g\,y}{v_s^2}, \quad \Xi = \frac{g\,x}{v_s^2}, \quad M = \frac{v}{v_s}, \quad \tau = \frac{g\,t}{v_s} H=vs2gy,Ξ=vs2gx,M=vsv,τ=vsgt

其中：

• vs=1116.4v_s = 1116.4vs=1116.4 ft/s 是海平面音速
• g=32.174g = 32.174g=32.174 ft/s² 是重力加速度
• MMM 就是马赫数

无量纲加速度（论文公式 8）：
AT=Tmax⁡ηmg=TηW,AD=Dmg=DW,AL=Lmg=LW A_T = \frac{T_{\max}\eta}{mg} = \frac{T\eta}{W}, \quad A_D = \frac{D}{mg} = \frac{D}{W}, \quad A_L = \frac{L}{mg} = \frac{L}{W} AT=mgTmaxη=WTη,AD=mgD=WD,AL=mgL=WL

注意这里 W=mg=34000W = mg = 34000W=mg=34000 lb 是重量 （不是质量），所以 AT,AL,ADA_T, A_L, A_DAT,AL,AD 都是无量纲的推重比/升重比/阻重比。

归一化因子：
X_scale=gvs2≈2.58×10−5 /ft \text{X\_scale} = \frac{g}{v_s^2} \approx 2.58 \times 10^{-5} \text{ /ft} X_scale=vs2g≈2.58×10−5 /ft

代码实现：

self.X_scale = self.g / self.a_sound**2  # g/v_s² ≈ 2.58e-5 /ft

样条插值的无量纲化：

代码中的 CasADiSplineInterpolator 支持在无量纲空间中构建样条：

self.spline_interp = CasADiSplineInterpolator(
    self.X_nodes, 
    bc_type='clamped',
    scale=self.X_scale,  # 传入归一化因子
    g=self.g,
    a_sound=self.a_sound
)

这样样条内部会自动进行转换：

• 无量纲距离节点：Ξi=Xi⋅X_scale\Xi_i = X_i \cdot \text{X\_scale}Ξi=Xi⋅X_scale
• 无量纲高度节点：Hi=hi⋅X_scaleH_i = h_i \cdot \text{X\_scale}Hi=hi⋅X_scale

关键性质：由于 HHH 和 Ξ\XiΞ 使用相同的归一化因子 s=g/vs2s = g/v_s^2s=g/vs2，导数自动正确：

dHdΞ=s⋅dhs⋅dX=dhdX(一阶导数不变) \frac{dH}{d\Xi} = \frac{s \cdot dh}{s \cdot dX} = \frac{dh}{dX} \quad \text{(一阶导数不变)} dΞdH=s⋅dXs⋅dh=dXdh(一阶导数不变)

d2HdΞ2=ddΞ(dHdΞ)=1s⋅d2hdX2=vs2g⋅d2hdX2 \frac{d^2H}{d\Xi^2} = \frac{d}{d\Xi}\left(\frac{dH}{d\Xi}\right) = \frac{1}{s} \cdot \frac{d^2h}{dX^2} = \frac{v_s^2}{g} \cdot \frac{d^2h}{dX^2} dΞ2d2H=dΞd(dΞdH)=s1⋅dX2d2h=gvs2⋅dX2d2h

但由于样条是在 (Ξ,H)(\Xi, H)(Ξ,H) 空间中构建的，evaluate_derivative(order=2) 直接返回 d2H/dΞ2d^2H/d\Xi^2d2H/dΞ2，无需额外转换！

注意 ：如果使用 SciPy 的 CubicSpline（在真实空间构建），则需要手动转换：

d2H_dXi2 = curvature_scale * d2h_dX2  # 仅当使用真实空间样条时需要

无量纲状态方程（论文公式 11）：
dMdτ=ATcos⁡α−AD−sin⁡γ \frac{dM}{d\tau} = A_T \cos\alpha - A_D - \sin\gamma dτdM=ATcosα−AD−sinγ

转换到以真实距离 XXX 为自变量：
dMdX=gvs2⋅ATcos⁡α−AD−sin⁡γMcos⁡γ=X_scale⋅ATcos⁡α−AD−sin⁡γMcos⁡γ \frac{dM}{dX} = \frac{g}{v_s^2} \cdot \frac{A_T \cos\alpha - A_D - \sin\gamma}{M \cos\gamma} = \text{X\_scale} \cdot \frac{A_T \cos\alpha - A_D - \sin\gamma}{M \cos\gamma} dXdM=vs2g⋅McosγATcosα−AD−sinγ=X_scale⋅McosγATcosα−AD−sinγ

代码实现：

dM_dX = self.X_scale * (A_T * ca.cos(alpha) - A_D - ca.sin(gamma)) / (M * ca.cos(gamma) + 1e-6)
M_new = M + dM_dX * self.dX

无量纲攻角方程（论文公式 37）：
ATsin⁡α+AL=(1+M2cos⁡2γ⋅d2HdΞ2)cos⁡γ A_T \sin\alpha + A_L = \left(1 + M^2 \cos^2\gamma \cdot \frac{d^2H}{d\Xi^2}\right) \cos\gamma ATsinα+AL=(1+M2cos2γ⋅dΞ2d2H)cosγ

由于样条自动返回无量纲曲率 d2H/dΞ2d^2H/d\Xi^2d2H/dΞ2，代码可以直接使用：

H_curr = self.spline_interp.evaluate(h_nodes, X_curr)      # 无量纲高度
dH_dXi = self.spline_interp.evaluate_derivative(..., 1)    # dH/dΞ
d2H_dXi2 = self.spline_interp.evaluate_derivative(..., 2)  # d²H/dΞ²

# 方程 (37) 直接使用无量纲曲率
rhs = (1 + M**2 * ca.cos(gamma)**2 * d2H_dXi2) * ca.cos(gamma)

真实物理量的使用：

虽然状态方程用无量纲形式，但动压、推力、升力、阻力仍然用真实物理量计算：

# 转回真实高度（用于动压和推力）
h_real = H_curr / self.X_scale

# 真实速度
V = M * self.a_sound

# 真实动压
q = 0.5 * rho * V**2

# 无量纲加速度（自动归一化）
A_T = eta * T_max / self.W   # T/W
A_D = D / self.W             # D/W

这种设计的优点是：

1. 状态方程形式与论文完全一致
2. 样条导数自动正确，无需手动转换曲率
3. 物理计算仍用熟悉的真实单位

1.5 无量纲状态与推力表的对应关系

前面我们分别介绍了：

• 优化中使用的无量纲状态 ：M,H,ΞM, H, \XiM,H,Ξ
• 推力表 Tmax⁡(M,h)T_{\max}(M, h)Tmax(M,h) 的定义：自变量是马赫数 MMM 和高度 hhh（单位 kft）

两者之间通过如下映射连接：

1. 马赫数与真实速度：
   V=M⋅vs V = M \cdot v_s V=M⋅vs

2. 无量纲高度与真实高度：
   H=ghvs2=h⋅X_scale⟹h=HX_scale H = \frac{g h}{v_s^2} = h \cdot \text{X\_scale} \quad\Longrightarrow\quad h = \frac{H}{\text{X\_scale}} H=vs2gh=h⋅X_scale⟹h=X_scaleH

3. 真实高度与推力表高度节点（kft）：
   hkft=h1000=H1000⋅X_scale h_{\text{kft}} = \frac{h}{1000} = \frac{H}{1000 \cdot \text{X\_scale}} hkft=1000h=1000⋅X_scaleH

因此，对任意无量纲状态 (M,H)(M, H)(M,H)，用于查表的物理自变量为：

(M,hkft)=(M,  H1000 X_scale) (M, h_{\text{kft}}) = \left(M, \; \frac{H}{1000\,\text{X\_scale}}\right) (M,hkft)=(M,1000X_scaleH)

在代码中，这一过程对应为（省略边界截断等细节）：

# 从无量纲样条得到 H_curr
H_curr = self.spline_interp.evaluate(h_nodes, X_curr)

# 1) 无量纲高度 → 真实高度 (ft)
h_real = H_curr / self.X_scale

# 2) 真实高度 → kft
h_kft = h_real / 1000.0

# 3) 真实速度由 M 得到（用于动压）
V = M * self.a_sound

# 4) 用 (M, h_kft) 在推力表中双线性插值
T_max = T_max_table(M, h_kft)

关键点：推力表本身保持在物理单位空间 （MMM, hkfth_{\text{kft}}hkft），不做无量纲化；

无量纲化只作用在状态方程和样条曲率上，通过上述映射桥接两者。

---

1.4 终端约束：以 NLP 形式写入

CasADi + IPOPT 要求问题写成
min⁡xf(x)s.t.lbg≤g(x)≤ubg,lbx≤x≤ubx \min_x f(x) \quad \text{s.t.}\quad lbg \le g(x) \le ubg,\quad lbx \le x \le ubx xminf(x)s.t.lbg≤g(x)≤ubg,lbx≤x≤ubx

在本问题中，典型的终端约束写成：

• 终端速度约束（允许 ±0.1\pm 0.1±0.1 ft/s 误差）：
  g1(x)=V(Xf)−Vf,−0.1≤g1(x)≤0.1 g_1(x) = V(X_f) - V_f,\qquad -0.1 \le g_1(x) \le 0.1 g1(x)=V(Xf)−Vf,−0.1≤g1(x)≤0.1

  代码：

  g.append(V_terminal - self.bc.V_f)
  lbg.append(-0.1)
  ubg.append(0.1)

• 终端高度约束（允许 ±50\pm 50±50 ft）：
  g2(x)=h(Xf)−hf,−50≤g2(x)≤50 g_2(x) = h(X_f) - h_f,\qquad -50 \le g_2(x) \le 50 g2(x)=h(Xf)−hf,−50≤g2(x)≤50

• 地形约束通过ζ(X)\zeta(X)ζ(X)的终端值软化为不等式：
  ζ(Xf)≥−ε \zeta(X_f) \ge -\varepsilon ζ(Xf)≥−ε

---

2. 立方样条插值：高度剖面h(X)h(X)h(X) 的精确参数化

代码中的 CasADiSplineInterpolator 是一个纯数学的立方样条实现 ，只依赖于 XXX节点，系数对 CasADi 符号变量 hih_ihi线性。

2.1 样条形式与光滑条件

在每个区间 Xi,Xi+1X_i, X_{i+1}Xi,Xi+1上，定义三次多项式
Si(x)=ai+bi(x−Xi)+ci(x−Xi)2+di(x−Xi)3 S_i(x) = a_i + b_i (x - X_i) + c_i (x - X_i)^2 + d_i (x - X_i)^3 Si(x)=ai+bi(x−Xi)+ci(x−Xi)2+di(x−Xi)3

要求满足：

1. 插值条件 ：
   Si(Xi)=hi,Si(Xi+1)=hi+1 S_i(X_i) = h_i,\qquad S_i(X_{i+1}) = h_{i+1} Si(Xi)=hi,Si(Xi+1)=hi+1
2. 一阶导连续 ：
   Si′(Xi+1)=Si+1′(Xi+1),i=0,...,n−2 S_i'(X_{i+1}) = S_{i+1}'(X_{i+1}),\quad i=0,\dots,n-2 Si′(Xi+1)=Si+1′(Xi+1),i=0,...,n−2
3. 二阶导连续 ：
   Si′′(Xi+1)=Si+1′′(Xi+1) S_i''(X_{i+1}) = S_{i+1}''(X_{i+1}) Si′′(Xi+1)=Si+1′′(Xi+1)
4. 边界条件 （这里采用 clamped ，端点一阶导数为0）：
   S0′(X0)=0,Sn−1′(Xn)=0 S_0'(X_0) = 0,\qquad S_{n-1}'(X_n) = 0 S0′(X0)=0,Sn−1′(Xn)=0

记 hi=h(Xi)h_i = h(X_i)hi=h(Xi)，区间长度为
hi(X)=Xi+1−Xi h_i^{(X)} = X_{i+1}-X_i hi(X)=Xi+1−Xi

代码中用 self.h = np.diff(self.X_nodes) 保存。

2.2 三对角方程的推导

经典立方样条推导告诉我们，可以先求解每个节点的"二阶导数系数" ci=Si′′(Xi)/2c_i = S_i''(X_i)/2ci=Si′′(Xi)/2。

对内部节点i=1,...,n−1i=1,\dots,n-1i=1,...,n−1，有方程
hi−1(X)ci−1+2(hi−1(X)+hi(X))ci+hi(X)ci+1=3(hi+1−hihi(X)−hi−hi−1hi−1(X)) h_{i-1}^{(X)} c_{i-1}+ 2 (h_{i-1}^{(X)} + h_i^{(X)}) c_i+ h_i^{(X)} c_{i+1} = 3 \left( \frac{h_{i+1}-h_i}{h_i^{(X)}} - \frac{h_i-h_{i-1}}{h_{i-1}^{(X)}} \right) hi−1(X)ci−1+2(hi−1(X)+hi(X))ci+hi(X)ci+1=3(hi(X)hi+1−hi−hi−1(X)hi−hi−1)

这正是代码中内部行的构造：

for i in range(1, n - 1):
    A[i, i-1] = h[i-1]
    A[i, i]   = 2 * (h[i-1] + h[i])
    A[i, i+1] = h[i]
    rhs[i] = 3 * ((h_nodes[i+1] - h_nodes[i]) / h[i]
                  - (h_nodes[i] - h_nodes[i-1]) / h[i-1])

对于 clamped 边界（两端导数为0），边界行变成：
2h0(X)c0+h0(X)c1=3h1−h0h0(X)hn−1(X)cn−1+2hn−1(X)cn=−3hn−hn−1hn−1(X) \begin{aligned} 2h_0^{(X)} c_0 + h_0^{(X)} c_1 &= 3\frac{h_1-h_0}{h_0^{(X)}} \\ h_{n-1}^{(X)} c_{n-1} + 2h_{n-1}^{(X)} c_n &= -3\frac{h_n-h_{n-1}}{h_{n-1}^{(X)}} \end{aligned} 2h0(X)c0+h0(X)c1hn−1(X)cn−1+2hn−1(X)cn=3h0(X)h1−h0=−3hn−1(X)hn−hn−1

对应代码：

A[0, 0] = 2 * h[0]
A[0, 1] = h[0]
...
A[n-1, n-2] = h[-1]
A[n-1, n-1] = 2 * h[-1]

rhs[0]   = 3 * (h_nodes[1]   - h_nodes[0]) / h[0]
rhs[n-1] = -3 * (h_nodes[n-1] - h_nodes[n-2]) / h[-1]

最终求解线性系统
{2h0(X)c0+h0(X)c1=3h1−h0h0(X),hi−1(X)ci−1+2(hi−1(X)+hi(X))ci+hi(X)ci+1=3(hi+1−hihi(X)−hi−hi−1hi−1(X)),i=1,...,n−2,hn−1(X)cn−1+2hn−1(X)cn=−3hn−hn−1hn−1(X). \begin{cases} 2h_0^{(X)} c_0 + h_0^{(X)} c_1 = 3\dfrac{h_1 - h_0}{h_0^{(X)}}, \\4pt h_{i-1}^{(X)} c_{i-1}+ 2\bigl(h_{i-1}^{(X)} + h_i^{(X)}\bigr) c_i+ h_i^{(X)} c_{i+1} = 3\left( \dfrac{h_{i+1}-h_i}{h_i^{(X)}} - \dfrac{h_i-h_{i-1}}{h_{i-1}^{(X)}} \right),\quad i=1,\dots,n-2, \\4pt h_{n-1}^{(X)} c_{n-1} + 2h_{n-1}^{(X)} c_n = -3\dfrac{h_n-h_{n-1}}{h_{n-1}^{(X)}}. \end{cases} ⎩ ⎨ ⎧2h0(X)c0+h0(X)c1=3h0(X)h1−h0,hi−1(X)ci−1+2(hi−1(X)+hi(X))ci+hi(X)ci+1=3(hi(X)hi+1−hi−hi−1(X)hi−hi−1),i=1,...,n−2,hn−1(X)cn−1+2hn−1(X)cn=−3hn−1(X)hn−hn−1.

A c=rhs A\, c = \text{rhs} Ac=rhs

在代码中使用预计算的 A_inv：

c = ca.mtimes(self.A_inv, rhs)

2.3 系数ai,bi,ci,dia_i,b_i,c_i,d_iai,bi,ci,di的显式公式

在得到了cic_ici后，可以写出其余系数：

ai=hibi=hi+1−hihi(X)−hi(X)3(2ci+ci+1)di=ci+1−ci3hi(X) \begin{aligned} a_i &= h_i \\ b_i &= \frac{h_{i+1} - h_i}{h_i^{(X)}} - \frac{h_i^{(X)}}{3}(2c_i + c_{i+1}) \\ d_i &= \frac{c_{i+1} - c_i}{3h_i^{(X)}} \end{aligned} aibidi=hi=hi(X)hi+1−hi−3hi(X)(2ci+ci+1)=3hi(X)ci+1−ci

完全对应代码：

a = h_nodes[:-1]
for i in range(n - 1):
    b[i] = (h_nodes[i+1] - h_nodes[i]) / h[i] - h[i] * (2*c[i] + c[i+1]) / 3
    d[i] = (c[i+1] - c[i]) / (3 * h[i])

样条值 和导数都可以用这些系数封装为简单多项式：

• 值：
  Si(x)=ai+biΔx+ciΔx2+diΔx3 S_i(x) = a_i + b_i \Delta x + c_i \Delta x^2 + d_i \Delta x^3 Si(x)=ai+biΔx+ciΔx2+diΔx3
• 一阶导：
  Si′(x)=bi+2ciΔx+3diΔx2 S_i'(x) = b_i + 2c_i \Delta x + 3d_i \Delta x^2 Si′(x)=bi+2ciΔx+3diΔx2
• 二阶导：
  Si′′(x)=2ci+6diΔx S_i''(x) = 2c_i + 6d_i \Delta x Si′′(x)=2ci+6diΔx

对应 evaluate 和 evaluate_derivative 两个方法。

---

3. 分段线性插值：油门历史 η(X)\eta(X)η(X)

油门的参数化更简单：在节点 (Xi,ηi)(X_i, \eta_i)(Xi,ηi)上做一次线性插值。

对于任意 XXX 落在区间 Xi,Xi+1X_i,X_{i+1}Xi,Xi+1，有：
η(X)=(1−t) ηi+t ηi+1,t=X−XiXi+1−Xi \eta(X) = (1-t)\,\eta_i + t\,\eta_{i+1},\quad t = \frac{X - X_i}{X_{i+1}-X_i} η(X)=(1−t)ηi+tηi+1,t=Xi+1−XiX−Xi

代码实现：

idx = np.searchsorted(self.X_nodes, X, side='right') - 1
idx = np.clip(idx, 0, self.n_nodes - 2)
X_left  = self.X_nodes[idx]
X_right = self.X_nodes[idx + 1]
t = (X - X_left) / (X_right - X_left)
eta = (1 - t) * eta_nodes[idx] + t * eta_nodes[idx + 1]

这就是标准的一维线性插值（又可以理解为线性有限元的形函数）。

在三阶段 Bang-Bang 优化中，我们进一步把ηi\eta_iηi固定为 0 或 1，仅优化切换点的横坐标，对应的数学形式见第 6 节。

---

4. 双线性插值：推力表 T(M,h)T(M,h)T(M,h)的构造

推力表在代码中以二维网格形式存储：
Tij=T(Mi,hj),Mi∈mach_table,hj∈altitude_table_kft T_{ij} = T(M_i, h_j),\quad M_i \in \text{mach\_table},\quad h_j \in \text{altitude\_table\_kft} Tij=T(Mi,hj),Mi∈mach_table,hj∈altitude_table_kft

4.1 数学形式：双线性插值

设某个点 (M,h)(M,h)(M,h) 落在 Mi,Mi+1×hj,hj+1M_i,M_{i+1} \times h_j,h_{j+1}Mi,Mi+1×hj,hj+1 中，记归一化坐标
λ=M−MiMi+1−Mi,μ=h−hjhj+1−hj \lambda = \frac{M - M_i}{M_{i+1} - M_i},\quad \mu = \frac{h - h_j}{h_{j+1} - h_j} λ=Mi+1−MiM−Mi,μ=hj+1−hjh−hj

则双线性插值给出
T(M,h)=(1−λ)(1−μ)Ti,j+λ(1−μ)Ti+1,j+(1−λ)μTi,j+1+λμTi+1,j+1 \begin{aligned} T(M,h) &= (1-\lambda)(1-\mu) T_{i,j} +\lambda(1-\mu) T_{i+1,j} \\ &\quad+ (1-\lambda)\mu T_{i,j+1}+ \lambda\mu T_{i+1,j+1} \end{aligned} T(M,h)=(1−λ)(1−μ)Ti,j+λ(1−μ)Ti+1,j+(1−λ)μTi,j+1+λμTi+1,j+1

**本质上：**在每个小矩形内，对两个方向各做一次线性插值。

4.2 CasADi interpolant 的数据布局

代码中没有手工实现上述公式，而是调用：

thrust_values_flat = thrust_table_kilo_lb.flatten(order='F')
self.thrust_interp_casadi = ca.interpolant(
    'thrust_table', 'bspline',
    [mach_table.tolist(), altitude_table_kft.tolist()],
    thrust_values_flat.tolist(),
    {'degree': [1, 1]}
)

关键点：

关键点：

• order='F'：按 Fortran 列优先（列优先，column-major） 展开二维表，使得
  values i+Nmach⋅j =Tij \text{values}\\,i + N_{\\text{mach}}\\cdot j\\, = T_{ij} valuesi+Nmach⋅j=Tij
  即先把第 0 列的所有行拍平，再是第 1 列......这正是 CasADi interpolant 默认的读值方式；
• 'bspline' 加 degree=[1,1]：在二维情形下就是双线性插值。

如果这里误用 order='C'（行优先，row-major），就会把本来属于同一列的数据「打散」成别的列，导致插值在很多格点上用错值。

4.3 一个具体的小例子：2×2 表的展开方式

为了更直观，我们先看一个非常小的 2×2 表，假设行是 Mach，列是高度：

T=10203040,mach=0.0,1.0,  alt=0,10 (kft). T = \begin{bmatrix} 10 & 20 \\\\ 30 & 40 \end{bmatrix}, \quad \text{mach} = 0.0, 1.0,\; \text{alt} = 0, 10\,(\text{kft}). T= 10302040 ,mach=0.0,1.0,alt=0,10(kft).

在 NumPy 里可以写成：

thrust = np.array([
    [10.0, 20.0],  # M = 0.0, h = [0, 10]
    [30.0, 40.0],  # M = 1.0, h = [0, 10]
])

4.3.1 使用 F 顺序（正确写法）

vals_F = thrust.flatten(order='F')
print(vals_F)  # [10. 30. 20. 40.]

CasADi 会认为：

• values[0] = 10 对应 T0,0=T(M0,h0)T_{0,0} = T(M_0, h_0)T0,0=T(M0,h0)；
• values[1] = 30 对应 T1,0=T(M1,h0)T_{1,0} = T(M_1, h_0)T1,0=T(M1,h0)；
• values[2] = 20 对应 T0,1=T(M0,h1)T_{0,1} = T(M_0, h_1)T0,1=T(M0,h1)；
• values[3] = 40 对应 T1,1=T(M1,h1)T_{1,1} = T(M_1, h_1)T1,1=T(M1,h1)。

也就是说，按照列的方向一列一列排开，和我们原来矩阵里的物理含义一一对应。

4.3.2 使用 C 顺序（错误写法）

如果不小心写成：

vals_C = thrust.flatten(order='C')
print(vals_C)  # [10. 20. 30. 40.]

而 CasADi 仍然按「列优先」来解释这些值，就会得到：

• values[0] = 10 → 仍然被当成 T0,0T_{0,0}T0,0（还算对）；
• values[1] = 20 → 被当成 T1,0T_{1,0}T1,0，但我们原来希望 T1,0=30T_{1,0} = 30T1,0=30；
• values[2] = 30 → 被当成 T0,1T_{0,1}T0,1，但我们原来希望 T0,1=20T_{0,1} = 20T0,1=20；
• values[3] = 40 → 被当成 T1,1T_{1,1}T1,1（这格碰巧也对）。

结果就是，一半的格点值都错位了 。如果原始表里有很多 0（例如你推力表高 Mach、高高度用 0 表示「无效」），那么用错顺序之后，这些 0 就会被错误地填到本来有推力的区域，插值出来的 T(M,h)T(M,h)T(M,h) 会被严重低估。

你的工程代码中：

thrust_values_flat = self.thrust_table_kilo_lb.flatten(order='F')
self.thrust_interp_casadi = ca.interpolant(
    'thrust_table', 'bspline',
    [self.mach_table.tolist(), self.altitude_table_kft.tolist()],
    thrust_values_flat.tolist(),
    {'degree': [1, 1]}
)

这里用 order='F' 就是为了保证：

valuesi+Nmach⋅j=Ttable(Mi,hj) \text{values}i + N_{\\text{mach}}\\cdot j = T_{\text{table}}(M_i, h_j) valuesi+Nmach⋅j=Ttable(Mi,hj)

和 RectBivariateSpline(self.mach_table, self.altitude_table_kft, self.thrust_table_kilo_lb, ...) 里对行、列的含义保持一致。你在 verify_thrust_interp.py 里已经验证过：一旦把 order='F' 改成 order='C'，例如在 M=1.0,h=0M=1.0, h=0M=1.0,h=0 这样的精确节点上，NumPy 给出 37.9 kN，而 CasADi 插值会错误地给出 0------这就是顺序错导致的直接后果。

---

---

5. 逆动力学与速度方程 dMdX\frac{dM}{dX}dXdM

5.1 力的分解与无量纲加速度

代码中的力学计算分为两步：

第一步：真实物理量计算
Tmax⁡=T(M,h)(推力表插值)T=η Tmax⁡q=12ρV2(真实动压)L=qSCL,D=qSCD \begin{aligned} T_{\max} &= T(M,h) \quad \text{(推力表插值)} \\ T &= \eta\,T_{\max} \\ q &= \frac12 \rho V^2 \quad \text{(真实动压)}\\ L &= q S C_L,\quad D = q S C_D \end{aligned} TmaxTqL=T(M,h)(推力表插值)=ηTmax=21ρV2(真实动压)=qSCL,D=qSCD

第二步：转为无量纲加速度（论文公式 8）
AT=TW,AL=LW,AD=DW A_T = \frac{T}{W},\quad A_L = \frac{L}{W},\quad A_D = \frac{D}{W} AT=WT,AL=WL,AD=WD

注意：W=mg=34000W = mg = 34000W=mg=34000 lb 是重量 ，所以 AT,AL,ADA_T, A_L, A_DAT,AL,AD 是无量纲的推重比/升重比/阻重比。

代码实现：

# 真实速度（用于动压）
V = M * self.a_sound

# 真实动压
q = 0.5 * rho * V**2

# 推力和气动力
T_max = self._get_thrust_max(h_real, V)
T = eta * T_max
L = q * self.S_ref * C_L
D = q * self.S_ref * C_D

# 无量纲加速度（不乘 g！）
A_T = T / self.W   # 推重比
A_L = L / self.W   # 升重比
A_D = D / self.W   # 阻重比

5.2 力平衡与攻角方程（论文公式 37）

沿法向方向的力平衡，用无量纲形式写成：

ATsin⁡α+AL=(1+M2cos⁡2γ⋅d2HdΞ2)cos⁡γ A_T \sin\alpha + A_L = \left(1 + M^2 \cos^2\gamma \cdot \frac{d^2H}{d\Xi^2}\right)\cos\gamma ATsinα+AL=(1+M2cos2γ⋅dΞ2d2H)cosγ

由于样条在无量纲空间中构建，d2H/dΞ2d^2H/d\Xi^2d2H/dΞ2 直接可用！

注意到
AL=qSCLααW A_L = \frac{q S C_{L\alpha} \alpha}{W} AL=WqSCLαα

于是可以整理成关于 α\alphaα 的非线性方程：
k1sin⁡α+k2α+k3=0 k_1 \sin\alpha + k_2 \alpha + k_3 = 0 k1sinα+k2α+k3=0

其中
k1=ηTmax⁡W,k2=qSCLαW,k3=−(1+M2cos⁡2γ⋅d2HdΞ2)cos⁡γ k_1 = \frac{\eta T_{\max}}{W},\quad k_2 = \frac{q S C_{L\alpha}}{W},\quad k_3 = -\left(1 + M^2 \cos^2\gamma \cdot \frac{d^2H}{d\Xi^2}\right)\cos\gamma k1=WηTmax,k2=WqSCLα,k3=−(1+M2cos2γ⋅dΞ2d2H)cosγ

代码实现：

# 从无量纲样条获取
H_curr = self.spline_interp.evaluate(h_nodes, X_curr)      # 无量纲高度
dH_dXi = self.spline_interp.evaluate_derivative(..., 1)    # dH/dΞ
d2H_dXi2 = self.spline_interp.evaluate_derivative(..., 2)  # d²H/dΞ²

# 方程右端项（直接用无量纲曲率）
rhs = (1 + M**2 * ca.cos(gamma)**2 * d2H_dXi2) * ca.cos(gamma)

# 系数（无量纲形式）
k1 = eta_curr * T_max / self.W
k2 = q * self.S_ref * C_La / self.W
k3 = -rhs

随后使用牛顿迭代求解 α\alphaα：

alpha_guess = -k3 / (k1 + k2 + 1e-8)
alpha_iter = alpha_guess
for _ in range(3):
    f_alpha = k1 * ca.sin(alpha_iter) + k2 * alpha_iter + k3
    df_alpha = k1 * ca.cos(alpha_iter) + k2
    alpha_iter = alpha_iter - f_alpha / (df_alpha + 1e-8)

5.3 沿切向的马赫数微分方程（论文公式 11 → 34）

论文的无量纲形式（以无量纲时间 τ\tauτ 为自变量）：
dMdτ=ATcos⁡α−AD−sin⁡γ \frac{dM}{d\tau} = A_T \cos\alpha - A_D - \sin\gamma dτdM=ATcosα−AD−sinγ

转换到以真实距离 XXX 为自变量：
dMdX=dM/dτdX/dτ⋅dτdX=gvs2⋅ATcos⁡α−AD−sin⁡γMcos⁡γ \frac{dM}{dX} = \frac{dM/d\tau}{dX/d\tau} \cdot \frac{d\tau}{dX} = \frac{g}{v_s^2} \cdot \frac{A_T \cos\alpha - A_D - \sin\gamma}{M\cos\gamma} dXdM=dX/dτdM/dτ⋅dXdτ=vs2g⋅McosγATcosα−AD−sinγ

即：
dMdX=X_scale⋅ATcos⁡α−AD−sin⁡γMcos⁡γ \frac{dM}{dX} = \text{X\_scale} \cdot \frac{A_T \cos\alpha - A_D - \sin\gamma}{M\cos\gamma} dXdM=X_scale⋅McosγATcosα−AD−sinγ

代码实现：

# dM/dX = X_scale * (A_T*cosα - A_D - sinγ) / (M*cosγ)
dM_dX = self.X_scale * (A_T * ca.cos(alpha) - A_D - ca.sin(gamma)) \
        / (M * ca.cos(gamma) + 1e-6)

# 欧拉积分
M_new = M + dM_dX * self.dX

5.4 与真实速度的关系

积分过程中状态变量是马赫数 MMM，需要时可转回真实速度：
V=M⋅vs V = M \cdot v_s V=M⋅vs

终端约束检查时：

V_terminal = M_terminal * self.a_sound  # 转回 ft/s

这样就形成了一个完整的逆动力学系统，依赖于：

• 高度剖面 h(X)h(X)h(X)（通过无量纲样条得到）
• 油门历史 η(X)\eta(X)η(X)
• 空气动力模型和推力模型

---

6. CasADi 与 IPOPT 的具体用法

6.1 符号变量与决策向量

在 CasADi 中：

• ca.SX.sym('h', n) 创建长度为 (n) 的符号向量；
• x = ca.vertcat(h_nodes, eta_nodes) 将两个向量拼接成大的决策向量。

h_nodes = ca.SX.sym('h', n)     # 高度节点
eta_nodes = ca.SX.sym('eta', n) # 油门节点
x = ca.vertcat(h_nodes, eta_nodes)  # 总长度 2n

在 Bang-Bang 阶段则变成：

h_nodes   = ca.SX.sym('h', n)
X_switches = ca.SX.sym('X_sw', n_sw)
x = ca.vertcat(h_nodes, X_switches)

6.2 构建 NLP：x, f, g

CasADi 期望传入一个字典：

nlp = {
    'x': x,   # 决策变量
    'f': J,   # 目标函数
    'g': ca.vertcat(*g)  # 约束向量
}

随后创建求解器：

solver = ca.nlpsol('solver', 'ipopt', nlp, opts)

opts 中的典型 IPOPT 配置为：

opts = {
    'ipopt': {
        'max_iter': max_iterations,
        'print_level': 5,
        'tol': tolerance,
        'acceptable_tol': 1e-4,
        'warm_start_init_point': 'yes',
        'mu_strategy': 'adaptive',
        'nlp_scaling_method': 'gradient-based',
    },
    'print_time': True
}

6.3 变量边界与约束边界

变量边界 lbx, ubx 与约束边界 lbg, ubg 都是纯数值向量 ，与 x、g(x) 的维度对应。

示例：高度节点下界为地形 + 50 ft，上界为地形 + 500 ft：

for i in range(n):
    X_i = self.X_nodes[i]
    F_i = self.terrain.get_elevation(X_i)
    lbx.append(F_i + 50)
    ubx.append(F_i + 500)

终端速度约束则写入 g 中，并为其设置 lbg, ubg：

g.append(V_terminal - self.bc.V_f)
lbg.append(-0.1)
ubg.append(0.1)

最终求解调用：

sol = solver(
    x0=x0,
    lbx=lbx,
    ubx=ubx,
    lbg=lbg,
    ubg=ubg
)

sol['x'] 即为最优解，sol['f'] 为最优目标值。

---

7. Bang-Bang 精细化阶段：从分段线性到纯 0/1 控制

前两阶段优化给出的油门历史 η(2)(X)\eta^{(2)}(X)η(2)(X) 通常已经呈现出明显的 Bang-Bang 结构，但由于采样和优化的原因，很多节点值仍然处于 (0,1)(0,1)(0,1) 之间。本节的目标是：

• 从第二阶段的分段线性油门 中提取出有限个切换点；
• 以"高度节点 + 切换点位置"为新的决策变量，构建第三阶段 NLP；
• 通过 CasADi + IPOPT 优化，使油门真正收缩为纯 0/1 的 Bang-Bang 历史，同时保持终端约束和地形约束。

7.1 从分段线性油门中提取切换结构

第二阶段的油门形式为（分段线性插值）：
η(2)(X)=PLInterp{(Xi,ηi)}i=0n−1,ηi∈0,1. \eta^{(2)}(X) = \text{PLInterp}\bigl\{(X_i, \eta_i)\bigr\}_{i=0}^{n-1}, \quad \eta_i \in 0,1. η(2)(X)=PLInterp{(Xi,ηi)}i=0n−1,ηi∈0,1.

观察数值结果可以发现：大部分 ηi\eta_iηi 已经非常接近 0 或 1，只是在切换附近有一小段平滑过渡。

第三阶段的第一步，就是在这些过渡区内插值出油门从 0 到 1 或从 1 到 0 的精确切换位置。

具体做法：

• 选定阈值（例如 0.50.50.5），把油门区分为"低档 (η≈0)(\eta\approx 0)(η≈0)"与"高档 (η≈1)(\eta\approx 1)(η≈1)"；
• 对每一段 Xi,Xi+1X_i,X_{i+1}Xi,Xi+1，检查
  (ηi−0.5) (ηi+1−0.5)<0 (\eta_i-0.5)\,(\eta_{i+1}-0.5) < 0 (ηi−0.5)(ηi+1−0.5)<0
  若成立，则说明该段内发生了一次 0↔1 或 1↔0 的切换；
• 在线性假设下，过阈值位置为：
  t=0.5−ηiηi+1−ηi,Xsw=Xi+t(Xi+1−Xi). t = \frac{0.5 - \eta_i}{\eta_{i+1}-\eta_i},\quad X_{sw} = X_i + t (X_{i+1}-X_i). t=ηi+1−ηi0.5−ηi,Xsw=Xi+t(Xi+1−Xi).

扫描所有区间，得到有序的切换点序列：
Xsw,1<Xsw,2<⋯<Xsw,m, X_{sw,1} < X_{sw,2} < \dots < X_{sw,m}, Xsw,1<Xsw,2<⋯<Xsw,m,

并根据每一段的平均值或端点值，确定对应的油门段值
ηk∈{0,1},k=1,...,m+1. \eta_k \in \{0,1\},\quad k=1,\dots,m+1. ηk∈{0,1},k=1,...,m+1.

这一步把"高维的分段线性控制"压缩成"少量切换点 + 段值"的结构化控制。

在实现中由 extract_switching_structure(eta_nodes, X_nodes, threshold=0.5) 完成。

7.2 平滑 Bang-Bang 控制律（为 IPOPT 提供可导控制）

如果在 NLP 里直接使用 if-else 形式的硬切换，

η(X)={η1,X<Xsw,1η2,Xsw,1≤X<Xsw,2⋮ηm+1,X≥Xsw,m \eta(X) = \begin{cases} \eta_1, & X < X_{sw,1} \\ \eta_2, & X_{sw,1} \le X < X_{sw,2} \\ \vdots & \\ \eta_{m+1}, & X \ge X_{sw,m} \end{cases} η(X)=⎩ ⎨ ⎧η1,η2,⋮ηm+1,X<Xsw,1Xsw,1≤X<Xsw,2X≥Xsw,m

那么控制对切换点 Xsw,kX_{sw,k}Xsw,k 将是不可导 的，IPOPT 得不到合理的梯度信息。

因此第三阶段中采用 Sigmoid 对 Bang-Bang 进行光滑近似：

η(X)=η1+∑k=1m(ηk+1−ηk) σ ⁣(ks(X−Xsw,k)), \eta(X) = \eta_1 + \sum_{k=1}^{m} \bigl(\eta_{k+1} - \eta_k\bigr)\, \sigma\!\bigl(k_s (X - X_{sw,k})\bigr), η(X)=η1+k=1∑m(ηk+1−ηk)σ(ks(X−Xsw,k)),

其中
σ(z)=11+e−z,ks≈0.01. \sigma(z) = \frac{1}{1+e^{-z}},\quad k_s \approx 0.01. σ(z)=1+e−z1,ks≈0.01.

• 当 X≪Xsw,kX \ll X_{sw,k}X≪Xsw,k 时，σ(ks(X−Xsw,k))≈0\sigma(k_s (X-X_{sw,k})) \approx 0σ(ks(X−Xsw,k))≈0，第 kkk 次切换尚未发生；
• 当 X≫Xsw,kX \gg X_{sw,k}X≫Xsw,k 时，σ≈1\sigma \approx 1σ≈1，第 kkk 次切换已经完成；
• 随着 ksk_sks 增大，过渡区收缩，控制律逐渐逼近理想的 0/1 Bang-Bang。

代码实现（CasADi 符号版）大致如下：

def _bangbang_throttle(self, X_switches, X, smooth=True):
    if smooth:
        k = 0.01  # 平滑系数
        eta = float(self.eta_values[0])
        for i in range(self.n_switches):
            delta_eta = self.eta_values[i + 1] - self.eta_values[i]
            sigmoid = 1.0 / (1.0 + ca.exp(-k * (X - X_switches[i])))
            eta = eta + delta_eta * sigmoid
        return eta
    else:
        # 硬 Bang-Bang，用于事后数值仿真
        eta = self.eta_values[0]
        for i in range(self.n_switches):
            eta = ca.if_else(X >= X_switches[i], self.eta_values[i + 1], eta)
        return eta

• 第三阶段 NLP 使用 smooth=True 版本，保证 η(X)\eta(X)η(X) 对 Xsw,kX_{sw,k}Xsw,k 可导；
• 数值仿真与可视化时，可以切换成 smooth=False 版本，检验真正的 0/1 控制。

7.3 Bang-Bang 阶段的 NLP 建模

在 Bang-Bang 精细化阶段，决策变量变为：

x=h0,...,hn−1,Xsw,1,...,Xsw,m⊤ x = \begin{bmatrix} h_0, \dots, h_{n-1}, X_{sw,1}, \dots, X_{sw,m} \end{bmatrix}^\top x=h0,...,hn−1,Xsw,1,...,Xsw,m⊤

其中：

• hih_ihi：高度样条节点（物理高度 ft，内部自动无量纲化为 HiH_iHi）；
• Xsw,kX_{sw,k}Xsw,k：切换点横坐标，直接决定 Bang-Bang 控制的"形状"。

状态方程与逆动力学部分完全继承前面章节的内容：

• 马赫数状态方程：
  dMdX=X_scale⋅AT(M,h,η,α)cos⁡α−AD(M,h,α)−sin⁡γMcos⁡γ, \frac{dM}{dX} = \text{X\_scale} \cdot \frac{A_T(M,h,\eta,\alpha)\cos\alpha - A_D(M,h,\alpha) - \sin\gamma} {M\cos\gamma}, dXdM=X_scale⋅McosγAT(M,h,η,α)cosα−AD(M,h,α)−sinγ,
• 法向力平衡（攻角方程）：
  ATsin⁡α+AL=(1+M2cos⁡2γ⋅d2HdΞ2)cos⁡γ, A_T \sin\alpha + A_L = \left(1 + M^2 \cos^2\gamma \cdot \frac{d^2H}{d\Xi^2}\right)\cos\gamma, ATsinα+AL=(1+M2cos2γ⋅dΞ2d2H)cosγ,
  展开为 k1sin⁡α+k2α+k3=0k_1 \sin\alpha + k_2 \alpha + k_3 = 0k1sinα+k2α+k3=0，用牛顿法迭代求解；
• 高度与曲率由无量纲样条给出，油门由平滑 Bang-Bang 控制律给出。

目标函数与第二阶段一致：

J(x)=ϕ∫X0Xf1Vcos⁡γ dX+(1−ϕ)∫(h−F)2dX(Xf−X0)href2+100⋅Penalty(α), J(x) = \phi \int_{X_0}^{X_f} \frac{1}{V\cos\gamma}\,dX + (1-\phi)\frac{\int (h-F)^2 dX}{(X_f-X_0)h_{\text{ref}}^2} + 100 \cdot \text{Penalty}(\alpha), J(x)=ϕ∫X0XfVcosγ1dX+(1−ϕ)(Xf−X0)href2∫(h−F)2dX+100⋅Penalty(α),

约束保持不变，但对切换点增加了时间间隔/空间间隔约束（防止切换过密）：

{∣V(Xf)−Vf∣≤0.1 ft/s∣h(Xf)−hf∣≤50 ftXsw,k+1−Xsw,k≥1000 ft,∀k \begin{cases} |V(X_f) - V_f| \le 0.1 \ \text{ft/s} \\ |h(X_f) - h_f| \le 50 \ \text{ft} \\ X_{sw,k+1} - X_{sw,k} \ge 1000 \ \text{ft},\quad \forall k \end{cases} ⎩ ⎨ ⎧∣V(Xf)−Vf∣≤0.1 ft/s∣h(Xf)−hf∣≤50 ftXsw,k+1−Xsw,k≥1000 ft,∀k

在 CasADi 层面，这仍然是一个标准的 NLP，只不过"控制参数化方式"从节点值 ηi\eta_iηi 变成了"有限个切换点位置"。

7.4 数值仿真与结果可视化（硬 Bang-Bang 验证）

第三阶段求解得到最优的 (hi∗,Xsw,k∗)(h_i^*, X_{sw,k}^*)(hi∗,Xsw,k∗) 后，代码中会调用一个数值仿真函数 _simulate_trajectory，用硬 Bang-Bang 控制来验证整个方案的可行性。流程如下：

1. 重建高度剖面 ：用 SciPy CubicSpline 根据 hi∗h_i^*hi∗ 构造 h(X)h(X)h(X)；
2. 构造硬 Bang-Bang 油门 ：
   η(X)={η1,X<Xsw,1η2,Xsw,1≤X<Xsw,2...ηm+1,X≥Xsw,m \eta(X) = \begin{cases} \eta_1, & X < X_{sw,1} \\ \eta_2, & X_{sw,1} \le X < X_{sw,2} \\ \dots \\ \eta_{m+1}, & X \ge X_{sw,m} \end{cases} η(X)=⎩ ⎨ ⎧η1,η2,...ηm+1,X<Xsw,1Xsw,1≤X<Xsw,2X≥Xsw,m
3. 沿 XXX 积分马赫数方程与攻角方程 ：使用与 CasADi 中相同的逆动力学关系，但全部用 NumPy 计算，得到 M(X),V(X),h(X),γ(X),α(X)M(X), V(X), h(X), \gamma(X), \alpha(X)M(X),V(X),h(X),γ(X),α(X)；
4. 绘制四条关键曲线 ：
   • 高度-航程（含地形）：验证贴地程度与不穿地约束；
   • 速度-航程（含 V0,VfV_0, V_fV0,Vf 虚线）：验证终端速度精度；
   • 攻角-航程（含 αmin⁡,αmax⁡\alpha_{\min}, \alpha_{\max}αmin,αmax）：验证攻角包线；
   • 油门-航程（标注每个 Xsw,kX_{sw,k}Xsw,k）：直观展示纯 0/1 Bang-Bang 结构。

主程序最终将图像保存为：

plt.savefig('three_stage_bangbang_result.png', dpi=300)

通过观察这张图，可以一目了然地验证：

• 终端速度和终端高度是否满足优化约束；
• 攻角是否始终处于可行区间；
• 油门是否真正收缩成有限切换点的纯 0/1 Bang-Bang 历史；
• 高度是否紧贴地形且未发生穿地。

至此，三阶段优化从"可行初值 → 光滑最优解 → 纯 Bang-Bang 控制"的整个实现与验证过程就完整闭环了。

8. 小结

本文从代码 trajectory_optimizer_casadi.py 出发，系统地梳理了：

• 无量纲化体系 ：论文的归一化方法（H,Ξ,M,τH, \Xi, M, \tauH,Ξ,M,τ），以及如何在样条插值中实现自动归一化；
• 高度样条插值：三对角方程与系数公式，支持无量纲空间构建；
• 油门分段线性插值：简单但高效的控制参数化；
• 双线性推力插值 ：CasADi interpolant 的数据布局（order='F' 的重要性）；
• 无量纲逆动力学方程 ：马赫数微分方程 dM/dXdM/dXdM/dX 的推导，攻角方程的牛顿迭代求解；
• CasADi + IPOPT 实战：NLP 构建、变量边界、约束边界的设置；
• Bang-Bang 优化：如何将控制结构收缩为具有明确切换点的纯 0/1 控制。

核心归一化公式速查：

量	真实单位	无量纲形式	转换因子
距离	XXX (ft)	Ξ=X⋅g/vs2\Xi = X \cdot g/v_s^2Ξ=X⋅g/vs2	2.58×10−52.58 \times 10^{-5}2.58×10−5 /ft
高度	hhh (ft)	H=h⋅g/vs2H = h \cdot g/v_s^2H=h⋅g/vs2	同上
速度	VVV (ft/s)	M=V/vsM = V/v_sM=V/vs	马赫数
加速度	T,L,DT, L, DT,L,D (lb)	AT=T/WA_T = T/WAT=T/W 等	力/重量
曲率	d2h/dX2d^2h/dX^2d2h/dX2 (1/ft)	d2H/dΞ2=(vs2/g)⋅d2h/dX2d^2H/d\Xi^2 = (v_s^2/g) \cdot d^2h/dX^2d2H/dΞ2=(vs2/g)⋅d2h/dX2	38722 ft

如果你已经读到这里，相信你不仅能看懂这份代码，还可以把它作为模板，拓展到更复杂的最优控制问题------比如多段 Bang-Bang、带过载约束的机动飞行，甚至三维路径规划。

欢迎在 CSDN 上转载或讨论这套实现思路，如需更完整的推导或代码讲解，也可以在评论区留言。