高斯伪谱法求解OCP

最优控制问题(Optimal Control Problem)的数值求解方法主要有间接法和直接法。间接法是根据极大值原理或者动态规划得到最优性的一阶必要条件，然后使用数值求解得到最优控制和轨迹。

间接法的优点是解的精度高，满足最优性的一阶必要条件。 缺点是收敛域很小，对初值估计要求高，且难以估计协态变量初值。而且需要求解状态方程和协态方程，增加了计算量， 算法鲁棒性较差。 对于复杂的系统， 推导一阶必要条件特别困难。
直接法是将原最优控制问题转化为非线性规划问题，然后采用非线性规划算法求解。由于不需要推导一阶必要条件，所以应用方便。直接法根据离散化变量的不同，主要可以分为只离散控制变量、只离散状态变量和同时离散控制变量和状态变量三类。第一类方法由于需要进行数值积分，计算量较大，这种算法的代表是打靶法和多段打靶法；第二类方法是微分包含法，它只离散状态变量，将控制约束转化为状态约束，可以降低变量个数，但是对于非线性比较复杂的状态方程，难以求出控制变量关于状态的显式表达式；第三类方法同时将状态变量和控制变量进行离散化，微分方程转化为代数约束条件， 这一类算法收敛速度较快，并且收敛半径大。[1]

配点法也是一种直接法，其包括欧拉法、Runge-Kutta法、Hermit-Simpson法和伪谱法。其中伪谱法包括Chebyshev伪谱法、Legendre伪谱法、Gauss伪谱法和Radau伪谱法。伪谱法因其计算效率和计算精度上的优势、良好的收敛性以及较低的初值敏感度在最优控制领域求解算法中倍受关注[2]。

Pseudospectral methods are a class of direct collocation where the optimal control problem is transcribed to a nonlinear programming problem (NLP) by parameterizing the state and control using global polynomials and collocating the differential-algebraic equations using nodes obtained from a Gaussian quadrature.[3]

1. 数学基本理论

1.1 Lagrange Polynomials

​ 假设给定$N+1$个数据点$(x_0,y_0),(x_1,y_1),\cdots ,(x_N,y_N)$，可以得到一个显示的，阶次为$N$插值多项式，称为Lagrange Polynomials(拉格朗日多项式)。
$$P(x)=\sum _{i=0}^N{y}_i{L}_i(x_i)\text{\hspace{1em}(1-1)}$$
其中，$L_i(x)$是Lagrange Polynomial的基函数。
$$\begin{gathered} L_i(x)=\prod _{j=0 \\ j\ne i}^N\frac{x-x_j}{x_i-x_j},i=0,1,2,\cdots ,N\text{\hspace{1em}(1-2)} \end{gathered}$$

1.2 Legendre Polynomials

Legendre Polynomials(勒让德多项式)是勒让德微分方程的解，勒让德多项式$P_n(x)$是$n$阶多项式，可用罗德里格公式表示为：
$$P_n(x)=\frac{1}{2^{n}n!}\frac{d^n}{d{x}^n}[(x^2-1{)}^n]\text{\hspace{1em}(1-3)}$$
相邻三个Legendre Polynomials具有递推关系：
$$(n+1)P_{n+1}=(2n+1)x{P}_n-n{P}_{n-1}\text{\hspace{1em}(1-4)}$$
Legendre Polynomials的微分递推关系可以用来计算Legendre Polynomials积分：
$$(2n+1)P_n=\frac{d}{dx}[P_{n+1}-P_{n-1}]\text{\hspace{1em}(1-5)}$$

1.3 Collocation Points

有三种常用的配点：Legendre-Gauss(LG)，Legendre-Gauss-Radau(LGR)和Legendre-Gauss-Lobatto(LGL) 。这些配点是Legendre Polynomials的根或者是Legendre Polynomials和其微分的根的线性组合。LG、LGR和LGL的配点分别分布在$\tau \in (-1,1)$，$\tau \in [-1,1)$或者$\tau \in (-1,1]$、$\tau \in [-1,1]$。假设$N$是配点的数目，$P_N(\tau )$是$N$阶Legendre Polynomial多项式，则有：

	多项式的根	分布	伪谱法分类
$LG$	$P_N(\tau )$	$\tau \in (-1,1)$	$Lobatto$
$LGR$	$P_{N-1}(\tau )+P_N(\tau )$	$\tau \in [-1,1)$或者$\tau \in (-1,1]$	$Gauss$
$LGL$	${\dot{P}}_{N-1}(\tau )$	$\tau \in [-1,1]$	$Radau$

1.4 Quadrature

采用高斯型数值积分计算近似计算积分：
$${\int }_{{\tau }_0}^{{\tau }_f}f(\tau )d\tau \approx \sum _{i=1}^N{w}_{i}f({\tau }_i)\text{\hspace{1em}(1-6)}$$
其中，$w_i$是quadrature weights，${\tau }_i$是quadrature points或者notes。$f(\tau )$采用Lagrange Polynomial形式有：
$${\int }_{{\tau }_0}^{{\tau }_f}f(\tau )d\tau ={\int }_{{\tau }_0}^{{\tau }_f}\sum _{i=0}^N{L}_i({\tau }_i)f({\tau }_i)d\tau \text{\hspace{1em}(1-7)}$$
由公式$(1-6)$和$(1-7)$可以得到：
$$w_i={\int }_{{\tau }_0}^{{\tau }_f}{L}_i({\tau }_i)d\tau ={\int }_{{\tau }_0}^{{\tau }_f}\prod _{k=1,k\ne i}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k}d\tau ,i=1,2,\cdots ,N\text{\hspace{1em}(1-8)}$$
如果${\tau }_i$是Legendre Polynomials的collocation points，可以得到：
$$w_i=\frac{2}{1-{\tau }_i^2}[{\dot{P}}_N({\tau }_i){]}^2\text{\hspace{1em}(1-9)}$$
其中，${\dot{P}}_N(\tau )$是$N$阶Legendre Polynomials的微分，${\tau }_0=-1,{\tau }_f=1$。

除此之外，可以通过对公式$(1-8)$变形得到公式$(1-10)$，求解线性方程组得到$w$：
$$\begin{array}{rl}& \left[\begin{array}{cccc}{P}_0({\tau }_1)& P_0({\tau }_2)& \cdots & P_0({\tau }_N)\\ P_1({\tau }_1)& P_1({\tau }_2)& \cdots & P_1({\tau }_N)\\ ⋮& ⋮& \ddots & ⋮\\ P_{N-1}({\tau }_1)& P_{N-1}({\tau }_2)& \cdots & P_{N-1}({\tau }_N)\end{array}\right]\left[\begin{array}{c}{w}_1\\ w_2\\ ⋮\\ w_n\end{array}\right]\\ & =\left[\begin{array}{cccc}{P}_0({\tau }_1)& P_0({\tau }_2)& \cdots & P_0({\tau }_N)\\ P_1({\tau }_1)& P_1({\tau }_2)& \cdots & P_1({\tau }_N)\\ ⋮& ⋮& \ddots & ⋮\\ P_{N-1}({\tau }_1)& P_{N-1}({\tau }_2)& \cdots & P_{N-1}({\tau }_N)\end{array}\right]\left[\begin{array}{c}{\int }_{{\tau }_0}^{{\tau }_f}{\prod }_{k=1,k\ne 1}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k}d\tau \\ {\int }_{{\tau }_0}^{{\tau }_f}{\prod }_{k=1,k\ne 2}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k}d\tau \\ ⋮\\ {\int }_{{\tau }_0}^{{\tau }_f}{\prod }_{k=1,k\ne N}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k}d\tau \end{array}\right]\\ & =\left[\begin{array}{c}{\int }_{{\tau }_0}^{{\tau }_f}(({\prod }_{k=1,k\ne 1}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k})P_0({\tau }_1)+({\prod }_{k=1,k\ne 2}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k})P_0({\tau }_2)+\cdots +({\prod }_{k=1,k\ne N}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k})P_0({\tau }_n))d\tau \\ {\int }_{{\tau }_0}^{{\tau }_f}(({\prod }_{k=1,k\ne 1}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k})P_1({\tau }_1)+({\prod }_{k=1,k\ne 2}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k})P_1({\tau }_2)+\cdots +({\prod }_{k=1,k\ne N}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k})P_1({\tau }_n))d\tau \\ ⋮\\ {\int }_{{\tau }_0}^{{\tau }_f}(({\prod }_{k=1,k\ne 1}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k})P_{N-1}({\tau }_1)+({\prod }_{k=1,k\ne 2}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k})P_{N-1}({\tau }_2)+\cdots +({\prod }_{k=1,k\ne N}^N\frac{\tau -{\tau }_k}{{\tau }_i-{\tau }_k})P_{N-1}({\tau }_n))d\tau \end{array}\right]\\ & =\left[\begin{array}{c}{\int }_{{\tau }_0}^{{\tau }_f}{P}_0(\tau )d\tau \\ {\int }_{{\tau }_0}^{{\tau }_f}{P}_1(\tau )d\tau \\ ⋮\\ {\int }_{{\tau }_0}^{{\tau }_f}{P}_{N-1}(\tau )d\tau \end{array}\right]\end{array}\text{\hspace{1em}(1-10)}$$

1.5 Optimal Control Problem

最优控制问题($OCP$)可以简述为：对于一个受控系统，在满足约束条件下，寻找最优的控制量使性能指标最小。数学描述为：寻找控制变量$u(t)\in R^m$，使得下述性能指标最小：
$$\begin{gathered} J=\Phi (\mathbf{X}(t_0),t_0,\mathbf{X}(t_f),t_f)+{\int }_{t_0}^{t_f}L(\mathbf{X}(t),\mathbf{U}(t),t)dt \\ st.\{\begin{array}{l}\dot{x}(t)=f(\mathbf{X}(t),\mathbf{U}(t),t),t\in [t_0,t_f]\\ \Phi (\mathbf{X}(t_0),t_0,\mathbf{X}(t_f),t_f)=0\\ C(\mathbf{X}(t),\mathbf{U}(t),t)\le 0\end{array}\text{\hspace{1em}(1-11)} \end{gathered}$$

其中，$\mathbf{X}\in R^n,\mathbf{U}\in R^m$。

2. Gauss Pseudo Spectral Method

2.1 OCP模型转换

采用高斯伪谱法对$OCP$模型$(1-11)$进行离散。由于上述$Bolza$问题的时域范围为$[t_0,t_f]$，但是高斯伪谱法的离散点分布在区间$[-1,1)$，因此引入$\tau$首先将$Bolza$问题转化到定义在$[-1,1)$上的形式。
$$t=\frac{t_f-t_0}{2}\tau +\frac{t_f+t_0}{2}\text{\hspace{1em}(2-1)}$$
将公式$(2-1)$带入公式$(1-11)$得：
$$\begin{gathered} J=\Phi (\mathbf{X}({\tau }_0),{\tau }_0,\mathbf{X}({\tau }_f),{\tau }_f)+\frac{t_f-t_0}{2}{\int }_{{\tau }_0}^{{\tau }_f}L(\mathbf{X}(\tau ),\mathbf{U}(\tau ),\tau )d\tau \\ st.\{\begin{array}{l}\frac{d\mathbf{X}}{d\tau }=\frac{t_f-t_0}{2}f(\mathbf{X}(\tau ),\mathbf{U}(\tau ),\tau ),\tau \in [{\tau }_0,{\tau }_f]\\ \Phi (\mathbf{X}({\tau }_0),{\tau }_0,\mathbf{X}({\tau }_f),{\tau }_f)=0\\ C(\mathbf{X}(\tau ),\mathbf{U}(\tau ),\tau )\le 0,\tau \in [{\tau }_0,{\tau }_f]\end{array}\text{\hspace{1em}(2-2)} \end{gathered}$$

2.2 Collocation Points

高斯伪谱法是$OCP$模型在$LG$配点(collocation points)上进行离散化，这些配点是$N$个Legendre Polynomial的零点和配点$-1$组成，Legendre Polynomial零点位于开区间$(-1,1)$，加上配点$-1$就构成了区间$[-1,1)$，因此高斯伪谱法有$N+1$个配点，即${\tau }_0,{\tau }_1,{\tau }_2,\cdots ,{\tau }_N$，其中${\tau }_0=-1$。

2.3 状态变量离散

状态变量在$N+1$个配点上，采用Lagrange Polynomial插值拟合，可得：
$${\mathbf{X}}_j(\tau )\approx \sum _{i=0}^N{L}_i(\tau ){\mathbf{X}}_j({\tau }_i)=\sum _{i=0}^N{L}_i(\tau )x_{ij},j=0,1,\cdots ,n\text{\hspace{1em}(2-3)}$$
其中，${\mathbf{X}}_j$表示第$j$个状态变量，${\tau }_i$表示第$i$个配点，则连续的$\mathbf{X}$轨迹优化问题转换为离散状态变量：
$$\mathbf{X}\approx \left[\begin{array}{cccc}{x}_{0,0}& x_{0,1}& \cdots & x_{0,n-1}\\ x_{1,0}& x_{1,1}& \cdots & x_{1,n-1}\\ ⋮& ⋮& \ddots & ⋮\\ x_{N,0}& x_{N,1}& \cdots & x_{N,n-1}\end{array}\right]\text{\hspace{1em}(2-4)}$$
其中，$n$是状态变量的维数，$N+1$是配点的数目。

2.4 控制变量离散

控制变量在$N+1$个配点上，采用Lagrange Polynomial插值拟合，可得：
$${\mathbf{U}}_j(\tau )\approx \sum _{i=0}^N{L}_i(\tau ){\mathbf{U}}_j({\tau }_i)=\sum _{i=0}^N{L}_i(\tau )u_{ij},j=0,1,\cdots ,m\text{\hspace{1em}(2-5)}$$
其中，${\mathbf{U}}_j$表示第$j$个控制变量，${\tau }_i$表示第$i$个配点，则连续的$\mathbf{U}$轨迹优化问题转换为离散状态变量：
$$\mathbf{U}\approx \left[\begin{array}{cccc}{u}_{0,0}& u_{0,1}& \cdots & u_{0,m-1}\\ u_{1,0}& u_{1,1}& \cdots & u_{1,m-1}\\ ⋮& ⋮& \ddots & ⋮\\ u_{N,0}& u_{N,1}& \cdots & u_{N,m-1}\end{array}\right]\text{\hspace{1em}(2-6)}$$
其中，$m$是控制变量的维数，$N+1$是配点的数目。

2.5 微分方程离散

对公式$(2-3)$进行微分，并令$\tau ={\tau }_k$得：
$${\dot{\mathbf{X}}}_j({\tau }_k)=\sum _{i=0}^N{\dot{L}}_i({\tau }_k)x_{ij}=\sum _{i=0}^N{\mathbf{D}}_{ki}{x}_{ij},{\mathbf{D}}_{ki}={\dot{L}}_i({\tau }_k),k=1,2,\cdots ,N\text{\hspace{1em}(2-7)}$$
矩阵$\mathbf{D}$叫做Gauss Pseudospectral differentiation matrix，维数是$N\times (N+1)$。$OCP$模型的动力学微分约束方程可以离散为：
$$(\sum _{i=0}^N{\mathbf{D}}_{k,i}){\mathbf{X}}_j=\frac{t_f-t_0}{2}(\mathbf{A}{\mathbf{X}}_{j-1}^T+\mathbf{B}{\mathbf{U}}_{j-1}^T{)}^T,j=1,2,\cdots ,n-1;\text{\hspace{1em}(2-8)}$$

2.6 终端状态约束离散

根据$Gauss$积分公式，终端状态可以表示为：
$$\begin{array}{rl}{\mathbf{X}}_f& ={\mathbf{X}}_{N+1}\\ & ={\mathbf{X}}_0+\frac{t_f-t_0}{2}f(\mathbf{X}(\tau ),\mathbf{U}(\tau ),\tau )d\tau \\ & ={\mathbf{X}}_0+\frac{t_f-t_0}{2}\sum _{k=1}^N{w}_k(\mathbf{A}{\mathbf{X}}_{k-1}^T+\mathbf{B}{\mathbf{U}}_{k-1}^T{)}^T\end{array}\text{\hspace{1em}(2-9)}$$
其中，$k=1,2,\cdots ,N$，$w_k$是公式$(1-10)$计算得到的结果。${\mathbf{X}}_{N+1}$并不在公式$(2-4)$中。

2.7 优化目标离散

$$J=\Phi (\mathbf{X}({\tau }_0),{\tau }_0,\mathbf{X}({\tau }_f),{\tau }_f)+\frac{t_f-t_0}{2}\sum _{k=1}^N{w}_k{\mathbf{L}}_k\text{\hspace{1em}(2-10)}$$

通过高斯伪谱法离散化$OCP$模型，得到的离散化模型就可以使用$NLP$等数值优化求解器进行求解了。

3. References

[1] 求解含复杂约束非线性最优控制问题的改进Gauss 伪谱法.

[2] 数值法求解最优控制问题（四）——伪谱法

[3] An overview of three pseudospectral methods for the numerical solution of optimal control problems.