4. 利用伪谱法求解滑翔轨迹优化

任务定义

本案例使用开源伪谱法最优控制工具 PSOPT，求解一个二维滑翔飞行器的最短时间轨迹优化问题。飞行器在高度-速度平面内运动，受升力系数控制，需满足动压、过载和热流约束。目标是通过改变配点数（网格密度），观察求解精度与收敛行为的变化。

问题建模

状态变量：高度 h（m）、速度 v（m/s）、航迹角 γ（rad）。控制变量：升力系数 CL。动力学方程：

dh/dt = v·sin(γ)
dv/dt = -D/m - g·sin(γ)
dγ/dt = (L/(m·v)) - (g/v - v/(R+h))·cos(γ)

其中升力 L = 0.5·ρ·v²·S·CL，阻力 D = 0.5·ρ·v²·S·(CD0 + K·CL²)，大气密度 ρ = ρ0·exp(-h/H)。初始条件：h(0)=30000 m，v(0)=2000 m/s，γ(0)=0。终端条件：h(tf)=15000 m，v(tf)=800 m/s，γ(tf)=0。路径约束：动压 q = 0.5·ρ·v² ≤ 30000 Pa，过载 n = sqrt(L²+D²)/(m·g) ≤ 2.5。目标函数：最小化终端时间 J = tf。

PSOPT实现

PSOPT采用Legendre伪谱法，将连续时间最优控制问题离散为非线性规划（NLP），通过IPOPT求解。以下代码完整定义问题并求解：

// glide_opt.cpp
#include "psopt.h"

using namespace PSOPT;

// 物理常数
const double m = 1000.0;      // 质量 kg
const double S = 10.0;        // 参考面积 m²
const double g = 9.81;        // 重力加速度 m/s²
const double R = 6371000.0;   // 地球半径 m
const double rho0 = 1.225;    // 海平面密度 kg/m³
const double H = 7200.0;      // 大气标高 m
const double CD0 = 0.02;
const double K = 0.04;

// 大气密度
double rho(double h) { return rho0 * exp(-h/H); }

// 动力学方程
void dynamics(adouble* derivatives, adouble* path, adouble* states,
              adouble* controls, adouble* parameters, adouble& time,
              adouble* xad, int iphase, Workspace* workspace) {
    double h = states[0], v = states[1], gamma = states[2];
    double CL = controls[0];
    double rho_val = rho(h);
    double q = 0.5 * rho_val * v * v;
    double L = q * S * CL;
    double D = q * S * (CD0 + K * CL * CL);

    derivatives[0] = v * sin(gamma);
    derivatives[1] = -D/m - g * sin(gamma);
    derivatives[2] = (L/(m*v)) - (g/v - v/(R+h)) * cos(gamma);
}

// 端点条件
void endpoint_cost(int* stages, adouble* xad, adouble* phi,
                   adouble* psi, adouble* lambda, adouble* mu,
                   adouble* x0, adouble* xf, adouble* t0, adouble* tf,
                   adouble* x0_err, adouble* xf_err, adouble* t0_err,
                   adouble* tf_err, Workspace* workspace) {
    phi[0] = tf[0];  // 最小化终端时间
}

void events(int* stages, adouble* xad, adouble* psi,
            adouble* lambda, adouble* mu, adouble* x0, adouble* xf,
            adouble* t0, adouble* tf, adouble* x0_err, adouble* xf_err,
            adouble* t0_err, adouble* tf_err, Workspace* workspace) {
    // 初始条件
    psi[0] = x0[0] - 30000.0;   // h0
    psi[1] = x0[1] - 2000.0;    // v0
    psi[2] = x0[2] - 0.0;       // gamma0
    // 终端条件
    psi[3] = xf[0] - 15000.0;   // hf
    psi[4] = xf[1] - 800.0;     // vf
    psi[5] = xf[2] - 0.0;       // gammaf
}

// 路径约束
void path_constraints(adouble* c, adouble* states, adouble* controls,
                      adouble* parameters, adouble& time, adouble* xad,
                      int iphase, Workspace* workspace) {
    double h = states[0], v = states[1];
    double rho_val = rho(h);
    double q = 0.5 * rho_val * v * v;
    double CL = controls[0];
    double L = q * S * CL;
    double D = q * S * (CD0 + K * CL * CL);
    double n = sqrt(L*L + D*D) / (m * g);

    c[0] = q / 30000.0 - 1.0;   // 动压约束 ≤ 30000 Pa
    c[1] = n / 2.5 - 1.0;       // 过载约束 ≤ 2.5
}

int main() {
    // 定义问题
    int nphases = 1;
    int nstates = 3;
    int ncontrols = 1;
    int nparameters = 0;
    int npath = 2;
    int nevents = 6;

    // 配点数（可修改）
    int nps[1] = { 30 };   // 默认30个配点

    // 初始化
    Prob problem(nphases, nstates, ncontrols, nparameters, npath, nevents);
    problem.set_dynamics(dynamics);
    problem.set_endpoint_cost(endpoint_cost);
    problem.set_events(events);
    problem.set_path_constraints(path_constraints);

    // 设置阶段
    problem.set_phase_limits(0, 0.0, 100.0);  // 时间范围
    problem.set_state_limits(0, 0, 10000.0, 40000.0);   // h
    problem.set_state_limits(0, 1, 100.0, 3000.0);      // v
    problem.set_state_limits(0, 2, -0.5, 0.5);          // gamma
    problem.set_control_limits(0, 0, -0.5, 1.5);        // CL

    // 初始猜测
    problem.set_initial_guess(0, 0, 30000.0);   // h
    problem.set_initial_guess(0, 1, 2000.0);    // v
    problem.set_initial_guess(0, 2, 0.0);       // gamma
    problem.set_initial_guess_time(0, 0.0, 50.0);

    // 设置配点数
    problem.set_phase_grid(0, nps[0]);

    // 求解
    problem.solve();

    // 输出结果
    problem.print_solution();
    return 0;
}

编译与运行

# 依赖：PSOPT、IPOPT、ADOL-C、ColPack
g++ -O2 glide_opt.cpp -o glide_opt -I/usr/local/include/psopt \
    -L/usr/local/lib -lpsopt -lipopt -ladolc -lcolpack
./glide_opt

配点数对比实验

修改 nps[0] 的值分别为 10、20、30、40，重新编译运行，记录终端时间 tf 和约束违反情况。预期结果：配点数过少（如10）时，轨迹精度不足，约束可能轻微违反；配点数增加至30以上，tf 收敛到稳定值（约35~40秒区间），约束满足。配点数继续增加（如40），结果变化很小，但计算时间增长。

验证方法

运行程序后，观察输出中的 tf 值、状态轨迹和约束乘子。判定标准：

程序正常收敛，IPOPT退出状态为“Optimal Solution Found”；
终端条件误差小于1e-4（程序自动报告）；
路径约束在容差范围内满足（动压≤30000 Pa，过载≤2.5）；
不同配点数下 tf 变化趋势符合上述预期。

预期输出示例（配点数=30时）：

Optimal Solution Found

Final time: 37.82 s

Constraint violation: 2.3e-6

注：以上为基于问题设定的示例值，实际数值以运行结果为准。

配点效果示意

结果判定

通过上述对比实验，可以直观看到：配点数从10增加到30时，轨迹形状和终端时间发生明显变化（精度提升）；从30增加到40时，结果基本不变，说明问题已收敛。实际应用中建议从30个配点起步，逐步加密验证收敛性。若约束违反或收敛失败，应增加配点数或检查初始猜测是否合理。

📚 课程目录