高斯伪谱法在非线性轨迹优化中的应用：一个MATLAB实例

高斯伪谱法在非线性轨迹优化中的应用：一个MATLAB实例

三十年磨一剑，搞了一辈子航天控制，见过了太多花里胡哨的优化算法，也痛心于太多人只会“面向搜索引擎编程”。今天，咱们就来聊聊高斯伪谱法（GPM），这玩意儿看似高深，实则就是把连续最优控制问题巧妙地转化成非线性规划问题来求解。我会用一个非典型的例子，加上简洁的MATLAB代码，让你彻底明白它的精髓。

1. 高斯伪谱法的核心思想

说白了，GPM就是一种配点法。它选取一些特定的点（也就是高斯点）作为“代表”，在这些点上强制满足系统的动力学方程和约束条件。通过插值，将状态变量和控制变量用一组基函数（通常是拉格朗日多项式）来近似。这样一来，原本无限维的最优控制问题，就变成了一个有限维的非线性规划（NLP）问题，可以用现成的优化求解器来解决。

GPM的优势在于其精度高、收敛性好。由于高斯点是高斯积分的节点，因此使用GPM进行离散化可以达到很高的精度。此外，GPM将连续问题转化为代数问题，避免了直接求解微分方程的困难。

2. 算例：受约束的非线性质量弹簧阻尼系统轨迹优化

咱们不用倒立摆这种烂大街的例子。考虑一个质量弹簧阻尼系统，但它的弹簧力是非线性的，阻尼也是速度的非线性函数。更进一步，我们给它加上一些约束，让问题更有挑战性。

2.1 问题描述

假设一个质量为 $m$ 的物体，受到非线性弹簧力和非线性阻尼力的作用。其动力学方程如下：

$m \ddot{x} + f_d(\dot{x}) + f_s(x) = u$

其中，$x$ 是物体的位置，$u$ 是控制力，$f_d(\dot{x})$ 是非线性阻尼力，$f_s(x)$ 是非线性弹簧力。我们假设：

$f_d(\dot{x}) = b_1 \dot{x} + b_2 \dot{x}^3$

$f_s(x) = k_1 x + k_2 x^3$

目标是设计一个控制力 $u(t)$，使得物体在给定的时间 $t_0$ 到 $t_f$ 内，从初始状态 $x(t_0) = x_0$ 和 $\dot{x}(t_0) = v_0$ 运动到目标状态 $x(t_f) = x_f$ 和 $\dot{x}(t_f) = v_f$。同时，控制力的大小要受到约束：$|u(t)| \leq u_{max}$。

此外，我们还对位置进行约束：$x_{min} \leq x(t) \leq x_{max}$。

我们的目标是最小化控制能量：

$J = \int_{t_0}^{t_f} u(t)^2 dt$

2.2 GPM转化

1. 状态变量定义： $x_1 = x$, $x_2 = \dot{x}$，控制变量为 $u$。
2. 状态方程：
   $\dot{x}_1 = x_2$
   $\dot{x}_2 = (u - f_d(x_2) - f_s(x_1)) / m$
3. 时间归一化： 将时间区间 $[t_0, t_f]$ 归一化到 $[-1, 1]$，定义 $\tau = 2(t - t_0) / (t_f - t_0) - 1$。
4. 高斯点选取： 选择 $N$ 个高斯-勒让德-洛巴托（GLL）点 $\tau_i$，作为配点。
5. 变量离散化： 将状态变量和控制变量在GLL点上离散化：$x_1(\tau_i) \approx x_{1i}$, $x_2(\tau_i) \approx x_{2i}$, $u(\tau_i) \approx u_i$。
6. 微分矩阵： 构建微分矩阵 $D$，用于近似状态变量的导数：$\dot{x}i \approx \sum x_j$。}^{N} D_{ij
7. 代数约束： 将状态方程在GLL点上离散化，得到代数约束：
   $\sum_{j=1}^{N} D_{ij} x_{1j} = x_{2i}$
   $\sum_{j=1}^{N} D_{ij} x_{2j} = (u_i - f_d(x_{2i}) - f_s(x_{1i})) / m$
8. 边界约束： 强制满足初始状态和目标状态：$x_{1}(t_0) = x_0$, $x_{2}(t_0) = v_0$, $x_{1}(t_f) = x_f$, $x_{2}(t_f) = v_f$。
9. 控制约束： $|u_i| \leq u_{max}$。
10. 状态约束： $x_{min} \leq x_{1i} \leq x_{max}$。
11. 目标函数离散化： 使用高斯积分近似目标函数：$J \approx \sum_{i=1}^{N} w_i u_i^2$，其中 $w_i$ 是高斯权重。

2.3 MATLAB代码实现

% 参数设置
m = 1;      % 质量
k1 = 1;     % 线性弹簧系数
k2 = 0.5;   % 非线性弹簧系数
b1 = 0.5;   % 线性阻尼系数
b2 = 0.1;   % 非线性阻尼系数
t0 = 0;     % 初始时间
tf = 10;    % 最终时间
x0 = 0;     % 初始位置
v0 = 0;     % 初始速度
xf = 2;     % 目标位置
vf = 0;     % 目标速度
umax = 5;    % 控制力上限
xmin = -3;   % 位置下限
xmax = 3;   % 位置上限
N = 20;     % GLL点数量

% 计算GLL点和权重
[tau, w] = leggauss_lobatto(N);

% 构建微分矩阵
D = compute_differential_matrix(tau);

% 优化变量数量
nVars = 3*N; % x1, x2, u

% 目标函数
objective = @(x) w'*(x(2*N+1:3*N).^2); % 控制能量

% 非线性约束
nonlcon = @(x) constraints(x, m, k1, k2, b1, b2, D, tau, t0, tf, x0, v0, xf, vf, N);

% 线性约束 (控制力上下限和位置上下限)
A = [];
b = [];
Aeq = [];
beq = [];

lb = [-inf(2*N,1); -umax*ones(N,1)]; % 控制力下限
ub = [inf(2*N,1); umax*ones(N,1)];  % 控制力上限
lb(1:N) = max(lb(1:N), xmin); %位置下限
ub(1:N) = min(nub(1:N), xmax); %位置上限

% 初始猜测
x0_guess = zeros(nVars, 1);

% 优化求解
options = optimoptions('fmincon','Display','iter','Algorithm','sqp');
[x, fval] = fmincon(objective, x0_guess, A, b, Aeq, beq, lb, ub, nonlcon, options);

% 提取优化结果
x1 = x(1:N);
x2 = x(N+1:2*N);
u = x(2*N+1:3*N);

% 时间轴
t = (tau + 1)*(tf - t0)/2 + t0;

% 绘图
figure;
subplot(3,1,1);
plot(t, x1);
xlabel('Time (s)');
yLabel('Position (m)');
grid on;

subplot(3,1,2);
plot(t, x2);
xlabel('Time (s)');
yLabel('Velocity (m/s)');
grid on;

subplot(3,1,3);
plot(t, u);
xlabel('Time (s)');
yLabel('Control Force (N)');
grid on;

% GLL点计算函数
function [tau, w] = leggauss_lobatto(N)
% 计算高斯-勒让德-洛巴托点和权重
if N==1
    tau = 0;
    w = 2;
else
    tau = zeros(N,1);
    w = zeros(N,1);
    x0 = cos(pi*(0:N-1)'/N); 
    P = ones(N,3); % Legendre Polynomials
    dP = ones(N,3); % Derivatives of Legendre Polynomials

    for i=1:N
        x = x0(i);
        for k=1:20 % Newton-Raphson iterations
            P(i,1) = 1;
            P(i,2) = x;
            dP(i,1) = 0;
            dP(i,2) = 1;

            for j=2:N-1
                P(i,j+1) = ( (2*j-1)*x*P(i,j) - (j-1)*P(i,j-1) ) / j;
                dP(i,j+1) = ( (2*j-1)*( P(i,j) + x*dP(i,j) ) - (j-1)*dP(i,j-1) ) / j;
            end
            delta = ( x*P(i,N) - P(i,N-1) ) / ( N*P(i,N) );
            x = x - delta;
            if abs(delta)<eps, break, end
        end
        tau(i) = x;
        w(i) = 2 / (N*(N-1)*P(i,N)^2);
    end
end

% 排序
[tau,idx] = sort(tau);
w = w(idx);
end


% 微分矩阵计算函数
function D = compute_differential_matrix(tau)
% 构建微分矩阵
N = length(tau);
D = zeros(N, N);
for i = 1:N
    for j = 1:N
        if i == j
            D(i, j) = 0;
        else
            num = 1;
            den = 1;
            for k = 1:N
                if k ~= i
                    den = den * (tau(i) - tau(k));
                end
            end
            for k = 1:N
                if k ~= j
                    num = num * (tau(j) - tau(k));
                end
            end
            D(i, j) = den / num;
        end
    end
    sum_row = sum(D(i, :));
    D(i, i) = -sum_row;
end
end

% 约束函数
function [c, ceq] = constraints(x, m, k1, k2, b1, b2, D, tau, t0, tf, x0, v0, xf, vf, N)
% 非线性约束函数
x1 = x(1:N);
x2 = x(N+1:2*N);
u = x(2*N+1:3*N);

ceq1 = D*x1 - x2; % x1_dot = x2
ceq2 = D*x2 - (u - (b1*x2 + b2*x2.^3) - (k1*x1 + k2*x1.^3))/m; % x2_dot = f(x,u)

% 边界条件
ceq3 = x1(1) - x0;
ceq4 = x2(1) - v0;
ceq5 = x1(end) - xf;
ceq6 = x2(end) - vf;

ceq = [ceq1; ceq2; ceq3; ceq4; ceq5; ceq6];

% 没有不等式约束
c = [];
end

代码解释：

• leggauss_lobatto(N) 函数用于计算 N 个GLL点及其权重。这是GPM的核心步骤之一，决定了离散化的精度。
• compute_differential_matrix(tau) 函数用于构建微分矩阵。微分矩阵用于近似状态变量的导数，将微分方程转化为代数方程。
• constraints 函数用于定义非线性约束。包括动力学方程的约束、初始状态和目标状态的约束。
• 主程序中，使用 fmincon 函数求解非线性规划问题。fmincon 是MATLAB优化工具箱中常用的函数，可以用于求解带约束的非线性优化问题。这里我们使用了SQP算法，因为它通常在处理这类问题时表现良好。

2.4 优化结果分析

运行上述代码，可以得到状态变量和控制变量的轨迹。通过观察这些轨迹，我们可以分析系统的运动行为，并评估控制策略的性能。需要注意的是，GPM的收敛性和精度受到GLL点数量的影响。通常情况下，增加GLL点的数量可以提高精度，但也会增加计算量。

• 状态变量轨迹： 位置和速度随时间的变化曲线，反映了系统的运动状态。
• 控制变量轨迹： 控制力随时间的变化曲线，反映了控制策略的实施情况。
• 约束条件： 检查控制力是否满足约束条件 $|u(t)| \leq u_{max}$，以及位置是否满足约束条件 $x_{min} \leq x(t) \leq x_{max}$。

3. 反思与批判

高斯伪谱法确实是个好东西，它能把复杂的最优控制问题变成简单的非线性规划问题。但它也有局限性，比如对于高度非线性的系统，可能需要更多的配点才能保证精度。此外，GPM对初始猜测比较敏感，一个好的初始猜测可以加快收敛速度。

更重要的是，我发现现在很多年轻人，包括一些所谓的“专家”，只会用GPOPS-IIhttps://wenku.csdn.net/doc/4042baw9eb 这样的商业软件，根本不理解算法的底层原理。这是一种非常危险的现象。软件只是工具，理解算法的本质才是最重要的。否则，你永远只能做一些重复性的工作，无法真正解决实际问题。

未来的发展方向，我认为应该是在GPM的基础上，结合一些自适应的配点策略，以及一些更高效的优化算法。同时，也应该加强对算法底层原理的研究，培养更多真正懂算法的人才。只有这样，我们才能在航天控制领域取得更大的突破。

4. 结语

希望通过这个例子，你能够理解高斯伪谱法的核心思想，并能够用MATLAB实现它。记住，不要满足于“面向搜索引擎编程”，要深入理解算法的本质，才能真正掌握它。搞学术，来不得半点虚假！

附录

4.1 GLL点与微分矩阵计算的理论依据

高斯-勒让德-洛巴托（GLL）点是勒让德多项式的根，加上区间的两个端点。这些点具有特殊的性质，使得在高斯积分中，可以用较少的点获得较高的精度。微分矩阵的构建基于拉格朗日插值多项式，通过对插值多项式求导，可以得到状态变量导数的近似值。

4.2 关于fmincon函数的使用说明

fmincon是MATLAB优化工具箱中一个非常强大的函数，可以用于求解带约束的非线性优化问题。在使用fmincon时，需要定义目标函数、约束函数、变量的上下限，以及初始猜测。fmincon提供了多种优化算法，例如SQP、内点法等。选择合适的算法可以提高优化效率和精度。

4.3 代码可复用性说明

上面的代码为了方便理解，写得比较直接。在实际工程应用中，可以将一些重复使用的代码封装成函数，提高代码的可复用性。例如，可以将动力学方程、约束条件等封装成函数，方便在不同的优化问题中使用。

参数对比表：