高斯伪谱法

1 计算原理

1.1 最优控制问题

在现代控制系统中，最优控制问题通常描述为在满足一定约束条件下，最小化某个性能指标。其数学形式如下：

$$ \dot{X} = f(X, U, t) \\\\ g(X(0),U(0),t) \leq 0 \quad \text{或} \quad g(X(t_{f}), U(t_{f}), t_{f}) \leq 0 \\\\ c(X,U,t) \leq 0 \\\\ \text{约束条件：} \min J(X,U,t) $$

状态方程：描述系统动态，即状态变量微分与当前状态、控制输入和时间的关系。
边界约束：包括初始和终端时刻的状态与控制约束。
过程约束：在整个控制过程中必须满足的强约束条件。
性能指标：控制系统设计目标，即最小化某个泛函 $J$。

1.2 性能指标类型

从变分法角度看，最优控制问题本质是求解泛函 $X(\cdot)$ 和 $U(\cdot)$，使性能指标 $J$ 最小。常见性能指标分为三类：

1.2.1 Mayer型性能指标

仅与终端状态相关：

$$ J_M(X(\cdot), U(\cdot)) = h(X(t_f)) $$

适用于只关注最终结果的问题，如导弹命中精度。

1.2.2 Lagrange型性能指标

关注整个控制过程：

$$ J_L(X(\cdot), U(\cdot)) = \int^{t_f}_{0} f^0(t, X(t), U(t)) \, dt $$

适用于需要优化整个过程的问题，如能耗最小化。

1.2.3 Bolza型性能指标

结合终端和过程性能：

$$ J_B(X(\cdot), U(\cdot)) = h(X(t_f)) + \int^{t_f}_{0} f^0(t, X(t), U(t)) \, dt $$

最常见的形式，兼顾最终状态和过程消耗。

2 Gauss积分回顾

2.1 基本概念

Gauss积分是一种数值积分方法，通过选取非均匀分布的积分点实现高精度积分。其核心思想是：使用 $n+1$ 个Gauss点可获得 $2n+1$ 阶代数精度。

与传统Newton-Cotes方法（如梯形法则）相比：

梯形法则（2点）仅具1阶精度：

$$ \int^{x_1}_{x_0} f(x) \, dx = \frac{h}{2} (f(x_0) + f(x_1)) - \frac{h^3}{12} f''(x) $$

Gauss积分公式：

$$ \int_{-1}^{1} f(x) \, dx = \sum_{i=1}^{n} c_i f(x_i) $$

其中 $c_i$ 为权重系数，$x_i$ 为Gauss点。

2.2 Gauss点选取原理

Gauss点的选取基于正交多项式理论：

$$ \int^b_a p_j(x) p_k(x) \, dx = \begin{cases} 0, & j \neq k \\ \neq 0, & j = k \end{cases} $$$$ p_i(x) = \frac{1}{2^i i!} \frac{d^i}{dx^i} [(x^2 - 1)^i] $$

在区间 $[-1, 1]$ 上正交。

$$ Q(x) = \sum_{i=1}^{n} L_i(x) f(x_i) $$$$ \int^1_{-1} Q(x) \, dx = \sum_{i=1}^{n} c_i f(x_i), \quad c_i = \int^1_{-1} L_i(x) \, dx $$

这种选点方式能最大化积分精度。

3 非线性规划（NLP）问题

非线性规划问题通式为：

$$ minimize \ f(x) \\\\ subject \ to \ g_i(x) \le 0 \ for \ each \ i \in \{1, \cdots , m\} \\\\ h_j(x) = 0 \ for \ each \ j \in \{1, \cdots , p\} \\\\ x \in X. $$$$ 最小化\ f(x)=x_1x_2 \\\\ 其中满足 \ g_1(x)=-x_1, g_2(x_2) = -x_2 \\\\ x \in R^2 $$

常用求解工具包括MATLAB的 fmincon、IPOPT、SNOPT等。高斯伪谱法的核心就是将最优控制问题转化为NLP问题求解。

4 Gauss伪谱法

4.1 时间区间变换

$$ t = \frac{(t_f - t_0) \tau + (t_f + t_0)}{2}, \quad \tau = \frac{2t - t_f - t_0}{t_f - t_0} $$

4.2 状态方程离散化

$$ \frac{dX}{dt} = f(X(t), U(t), t) $$$$ \frac{dX}{d\tau} \frac{d\tau}{dt} = f(X(\tau), U(\tau), \tau) $$$$ \frac{dX}{d\tau} = \frac{t_f - t_0}{2} f(X(\tau), U(\tau), \tau) $$$$ X(\tau) \approx \sum_{i=0}^{n} L_i(\tau) X(\tau_i), \quad U(\tau) \approx \sum_{i=1}^{n} L_i^*(\tau) U(\tau_i) $$$$ L_i(\tau) = \prod_{j=0, j \neq i}^{n} \frac{\tau - \tau_j}{\tau_i - \tau_j}, \quad L_i^*(\tau) = \prod_{j=1, j \neq i}^{n} \frac{\tau - \tau_j}{\tau_i - \tau_j} $$$$ \dot{X}(\tau_k) \approx \sum_{i=0}^{n} D_{ki} X(\tau_i), \quad D_{ki} = \dot{L}_i(\tau_k) $$$$ \sum_{i=0}^{n} D_{ki} X_i - \frac{t_f - t_0}{2} F(X_k, U_k, \tau_k; t_0, t_f) = 0 $$

5 计算例程

$$ \dot{x} = -x^2 + u, \quad x(0) = 1, \quad x(2) = 0.5, \quad J = 0.5 \int^2_0 u^2(t) \, dt, \quad t \in [0, 2] $$

5.1 问题变换

$$ t = \tau + 1 \Rightarrow \frac{dx}{d\tau} = -x^2 + u $$$$ \tau_0 = -1, \quad \tau_1 = -\frac{1}{\sqrt{3}}, \quad \tau_2 = \frac{1}{\sqrt{3}}, \quad \tau_f = 1 $$

5.2 离散化

$$ X(\tau) \approx L_0(\tau) X_0 + L_1(\tau) X_1 + L_2(\tau) X_2 \\\\ U(\tau) \approx L_1^*(\tau) U_1 + L_2^*(\tau) U_2 $$$$ Z = [X_0, X_1, X_2, U_1, U_2]^T $$$$ D = \begin{bmatrix} \frac{(\tau_1 - \tau_1) + (\tau_1 - \tau_2)}{(\tau_0 - \tau_1)(\tau_0 - \tau_2)} & \frac{(\tau_1 - \tau_0) + (\tau_1 - \tau_2)}{(\tau_1 - \tau_0)(\tau_1 - \tau_2)} & \frac{(\tau_1 - \tau_0) + (\tau_1 - \tau_1)}{(\tau_2 - \tau_0)(\tau_2 - \tau_1)} \\\\ \frac{(\tau_2 - \tau_1) + (\tau_2 - \tau_2)}{(\tau_0 - \tau_1)(\tau_0 - \tau_2)} & \frac{(\tau_2 - \tau_0) + (\tau_2 - \tau_2)}{(\tau_1 - \tau_0)(\tau_1 - \tau_2)} & \frac{(\tau_2 - \tau_0) + (\tau_2 - \tau_1)}{(\tau_2 - \tau_0)(\tau_2 - \tau_1)} \end{bmatrix} $$

约束条件：

初始条件：$X_0 = 1$
终端条件：$X_f \approx X_0 + \sum_{i=1}^2 c_i (-X_i^2 + U_i)$
微分约束：

$$ \begin{cases} D_{00} X_0 + D_{01} X_1 + D_{02} X_2 = -X_1^2 + U_1 \\\\ D_{10} X_0 + D_{11} X_1 + D_{12} X_2 = -X_2^2 + U_2 \end{cases} $$

目标函数：$\min J = 0.5 (U_1^2 + U_2^2)$

5.3 MATLAB实现

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101


clear, clc

% 计算Gauss-Legendre节点和权重
function [x, w] = lgwt(N, a, b)
    % 本函数计算N阶Gauss-Legendre节点x和权重w在区间[a,b]上
    N = N-1;
    N1 = N+1; N2 = N+2;
    xu = linspace(-1, 1, N1)';
    y = cos((2*(0:N)'+1)*pi/(2*N+2)) + (0.27/N1)*sin(pi*xu*N/N2);
    L = zeros(N1, N2);
    Lp = zeros(N1, N2);
    y0 = 2;
    while max(abs(y-y0)) > eps
        L(:,1) = 1; Lp(:,1) = 0;
        L(:,2) = y; Lp(:,2) = 1;
        for k = 2:N1
            L(:,k+1) = ( (2*k-1)*y.*L(:,k) - (k-1)*L(:,k-1) ) / k;
        end
        Lp = (N2)*( L(:,N1) - y.*L(:,N2) )./(1-y.^2);
        y0 = y;
        y = y0 - L(:,N2)./Lp;
    end
    x = (a*(1-y) + b*(1+y))/2;
    w = (b-a)./((1-y.^2).*Lp.^2)*(N2/N1)^2;
end

% 主程序
[x, c] = lgwt(2, -1, 1); % 获取Gauss点x和权重c
tau = [-1; flip(x); 1];   % 包含初始和终端点

% 计算微分矩阵D
D = zeros(2, 3); % 2个配点（tau1, tau2），3个状态点（tau0, tau1, tau2）
for k = 1:2
    for i = 1:3
        prod_num = 1;
        prod_den = 1;
        for j = 1:3
            if j ~= i
                prod_num = prod_num * (tau(k+1) - tau(j));
                prod_den = prod_den * (tau(i) - tau(j));
            end
        end
        D(k, i) = prod_num / prod_den;
    end
end

% 优化求解
x0 = zeros(5, 1); % 初始猜测 [X0, X1, X2, U1, U2]
options = optimoptions('fmincon', 'Display', 'iter');
x_opt = fmincon(@(x)myfun(x), x0, [], [], [], [], [], [], @(x)mycon(x, D), options);

% 提取结果
X0 = x_opt(1);
X1 = x_opt(2);
X2 = x_opt(3);
U1 = x_opt(4);
U2 = x_opt(5);

% 绘制状态和控制曲线
L0 = @(t) (t - tau(2)).*(t - tau(3))./((tau(1) - tau(2))*(tau(1) - tau(3)));
L1 = @(t) (t - tau(1)).*(t - tau(3))./((tau(2) - tau(1))*(tau(2) - tau(3)));
L2 = @(t) (t - tau(1)).*(t - tau(2))./((tau(3) - tau(1))*(tau(3) - tau(2)));
LU1 = @(t) (t - tau(3))./(tau(2) - tau(3));
LU2 = @(t) (t - tau(2))./(tau(3) - tau(2));

fx = @(t) L0(t).*X0 + L1(t).*X1 + L2(t).*X2;
fu = @(t) LU1(t).*U1 + LU2(t).*U2;

figure;
subplot(2, 1, 1);
fplot(fx, [-1, 1]);
title('状态变量 x(τ)');
xlabel('τ'); ylabel('x');

subplot(2, 1, 2);
fplot(fu, [-1, 1]);
title('控制变量 u(τ)');
xlabel('τ'); ylabel('u');

% 目标函数
function f = myfun(x)
    U1 = x(4);
    U2 = x(5);
    f = 0.5 * (U1^2 + U2^2);
end

% 约束函数
function [c, ceq] = mycon(x, D)
    X0 = x(1);
    X1 = x(2);
    X2 = x(3);
    U1 = x(4);
    U2 = x(5);
    
    c = []; % 无不等式约束
    ceq = zeros(4, 1);
    ceq(1) = X0 - 1; % 初始条件
    ceq(2) = X0 + (1 * (-X1^2 + U1) + 1 * (-X2^2 + U2)) - 0.5; % 终端条件
    ceq(3) = D(1,1)*X0 + D(1,2)*X1 + D(1,3)*X2 + X1^2 - U1; % 配点1约束
    ceq(4) = D(2,1)*X0 + D(2,2)*X1 + D(2,3)*X2 + X2^2 - U2; % 配点2约束
end