1. 建立飞行器轨迹优化的基本工作流

任务定义

本案例将构建一个完整的飞行器轨迹优化最小工作流：给定飞行器从起点到终点的水平位移、飞行时间与初始/终端速度约束，以最小化能量消耗为目标，使用三次多项式参数化垂直轨迹，并通过SciPy的minimize求解最优参数，最终绘制优化前后的轨迹对比图。

问题建模

设飞行器在水平方向匀速运动，水平速度 Vx = 100 m/s，总飞行时间 T = 10 s，水平位移 L = 1000 m。垂直方向位移 y(t) 用三次多项式表示：

y(t) = a0 + a1·t + a2·t² + a3·t³

边界约束：起点高度 y(0)=0，终点高度 y(T)=0，起点垂直速度 y'(0)=0，终点垂直速度 y'(T)=0。由这四个约束可唯一消去 a0, a1, a2, a3，仅剩一个自由参数 a3（实际为中间点抬升高度）。优化目标为最小化垂直方向加速度平方的积分（表征能量消耗）：

J = ∫₀ᵀ [y''(t)]² dt

通过解析推导，y''(t) = 6·a3·t + 2·a2，代入边界条件后目标函数可化为关于 a3 的二次函数，最优解存在解析值。但为演示通用优化工作流，我们仍使用SciPy数值求解。

代码实现

import numpy as np
from scipy.optimize import minimize
import matplotlib.pyplot as plt

# 飞行参数
Vx = 100.0      # 水平速度 m/s
T = 10.0        # 飞行时间 s
L = Vx * T      # 水平位移 m

# 边界条件: y(0)=0, y(T)=0, y'(0)=0, y'(T)=0
# 三次多项式 y(t)=a0+a1*t+a2*t^2+a3*t^3
# 由边界条件消去 a0,a1,a2，仅剩自由参数 a3
def compute_coeffs(a3):
    """根据自由参数 a3 计算多项式系数 [a0,a1,a2,a3]"""
    a2 = -1.5 * a3 * T          # 由 y'(T)=0 推出
    a1 = 0.0                    # 由 y'(0)=0
    a0 = -a2 * T**2 - a3 * T**3 # 由 y(T)=0
    return np.array([a0, a1, a2, a3])

def y_t(t, coeffs):
    """计算多项式在 t 处的值"""
    return coeffs[0] + coeffs[1]*t + coeffs[2]*t**2 + coeffs[3]*t**3

def y2_t(t, coeffs):
    """计算二阶导数 y''(t)"""
    return 2*coeffs[2] + 6*coeffs[3]*t

# 目标函数: 加速度平方的积分 (能量消耗)
def objective(a3):
    coeffs = compute_coeffs(a3[0])
    # 使用高斯-勒让德数值积分 (5点)
    t_pts, w_pts = np.polynomial.legendre.leggauss(5)
    # 映射到 [0, T]
    t_map = 0.5*T*(t_pts + 1)
    w_map = 0.5*T*w_pts
    accel = np.array([y2_t(t, coeffs) for t in t_map])
    return np.sum(w_map * accel**2)

# 初始猜测
x0 = np.array([1.0])

# 求解
res = minimize(objective, x0, method='BFGS')
a3_opt = res.x[0]
coeffs_opt = compute_coeffs(a3_opt)

# 输出结果
print(f"优化成功: {res.success}")
print(f"最优 a3 = {a3_opt:.6f}")
print(f"多项式系数 [a0,a1,a2,a3] = {np.round(coeffs_opt, 4)}")
print(f"最小能量指标 J = {objective(res.x):.6f}")

# 解析解验证 (由 dJ/da3=0 推出)
a3_analytic = 0.0  # 实际解析解为0 (直线轨迹)
print(f"解析最优 a3 = {a3_analytic} (直线飞行)")

# 可视化
t_dense = np.linspace(0, T, 100)
y_opt = [y_t(t, coeffs_opt) for t in t_dense]
# 对比: 未优化的初始猜测 (a3=1.0)
coeffs_init = compute_coeffs(1.0)
y_init = [y_t(t, coeffs_init) for t in t_dense]

plt.figure(figsize=(8, 4))
plt.plot(t_dense, y_init, '--', label='初始猜测 (a3=1.0)', alpha=0.7)
plt.plot(t_dense, y_opt, '-', linewidth=2, label='优化轨迹 (a3=0)')
plt.xlabel('时间 t (s)')
plt.ylabel('垂直位移 y (m)')
plt.title('飞行器垂直轨迹优化结果')
plt.legend()
plt.grid(True, alpha=0.3)
plt.savefig('trajectory_opt.png', dpi=100)
print("轨迹图已保存为 trajectory_opt.png")

运行与验证

依赖安装：需要 numpy, scipy, matplotlib。执行 pip install numpy scipy matplotlib 即可。

运行方式：将上述代码保存为 trajectory_opt.py，在终端执行 python trajectory_opt.py。

预期输出：程序应打印优化成功标志、最优 a3≈0.0（直线轨迹），并生成 trajectory_opt.png 图像。图像中优化轨迹为一条从 (0,0) 到 (10,0) 的水平直线，而初始猜测为弯曲曲线。

判定标准：观察终端输出中 res.success 为 True，且 a3_opt 接近 0（误差小于 1e-4）。图像文件成功生成且优化轨迹为直线，即表示工作流闭环验证通过。

结果分析

优化结果表明，在给定边界条件下，最小能量轨迹为直线飞行（a3=0），此时垂直加速度恒为零，能量消耗为零。这与物理直觉一致：无中间约束时，直线轨迹最省能量。本案例通过最小可复现实现，完整演示了“问题定义→参数化→数值求解→结果验证”的轨迹优化标准工作流，该流程可直接扩展到更复杂的约束（如障碍物规避、动力学模型）与更高阶的参数化方法。