最优控制理论 二+、哈密尔顿函数法的补充

前面我在第二章最优控制理论 二、哈密尔顿函数法给出了Hamilton函数法一些重要推导过程和一些常用公式。最近翻看，觉得写得太多了，于是把一部分不重要的贴到下面，另成一篇。

2. 其他等式约束的转化

第3部分我们只考虑了终端等式约束，但是实际的动力学和控制问题里常有其他类型的等式约束，如

• 积分方程约束${\int }_0^{t_f}N(x(t),u(t),t)\text{d}t=\beta$
• 控制输入的约束$N(u(t),t)=0$
• 控制输入和状态变量的等式约束$N(x(t),u(t),t)=0$

下面可以证明，以上这些等式约束都可以用Hamilton函数法解决。

2.1 积分方程约束

对$${\int }_0^{t_f}N(x(t),u(t),t)\text{d}t=\beta \in {\mathbb{R}}^q\text{\hspace{1em}(积分方程约束)}$$引入扩充的状态变量$y(t)\in {\mathbb{R}}^q$且它满足
$$\begin{gathered} \dot{y}=N(x,u,t) \\ y(0)=0,y(t_f)={\int }_0^{t_f}N(x(t),u(t),t)\text{d}t\beta \text{\hspace{1em}(2)} \end{gathered}$$

则把上面这个积分方程约束化为终端状态约束，两个方程等价。仍可套用终端状态约束的框架，需要注意由于状态扩充为$n+q$维；并多了$q$个Lagrange乘乘数，加上原来的$m$个，共有$m+q$个终端约束和$m+q$个未知的Lagrange乘数。

2.2 控制变量和状态变量等式约束

设状态变量和等式约束具有以下形式：
$$N(x(t),u(t),t)=0,t_0<t<t_f\text{\hspace{1em}(状态+控制等式约束)}$$对这个q维的等式约束，引入Lagrange乘子$\mu (t)\in {\mathbb{R}}^q$，并且Hamilton函数变成：
$$H\triangleq L+{\lambda }^{\mathrm{T}}f+{\mu }^{\mathrm{T}}N\text{\hspace{1em}(3)}$$相应地，协态方程和控制方程都要变：
$$\begin{gathered} \dot{\lambda }=-\frac{\partial H}{\partial x}=-L_x-{\lambda }^{\mathrm{T}}{f}_x-{\mu }^{\mathrm{T}}{N}_x \\ \frac{\partial H}{\partial u}=L_u+{\lambda }^{\mathrm{T}}{f}_u+{\mu }^{\mathrm{T}}{N}_u=0 \end{gathered}$$

可见等式约束问题的处理方式都是一样的。
另外，纯控制变量约束$N_1(u(t),t)=0$和状态变量等式约束$N_2(x(t),t)=0$，都属于控制变量+状态变量等式约束的一种，可以直接套用上述方法。

3. 内点约束与角点条件

前面的假设是控制$u(t)$和状态变量$x(t)$都在$t_0<t<t_f$区间段连续可导，但是由于种种限制，往往很难做到这一点。下面我们来考虑协态变量$\lambda (t)$和Hamilton函数的连续性和可导性。

角点处的必要条件

前面的最优路径都假设极值曲线$x^{\ast }(t)$是连续可导的，但最优控制问题中由于动力学层面的约束使得理论上的最优轨迹连续但不是处处可导的，而是在某些角点出现：
$$x(t_1^{-})=x(t_1^{+});\dot{x}(t_1^{-})\ne \dot{x}(t_1^{+})$$
如下图所示的曲线
按照变分原理，由于极值曲线分段可导，则轨线分段满足Euler方程，
$$L_x-\frac{\text{d}}{\text{d}t}{L}_{\dot{x}}=0，t\in [0,t_1)\bigcup (t_1,t_f]$$

此外在角点处还满足必要条件：
KaTeX parse error: Unexpected end of input in a macro argument, expected '}' at position 69: …\ L(t_1^-)-\dot\̲m̲a̲t̲h̲b̲f̲ ̲x^TL_{\dot x}(t…

这个条件称为Weierstrass-Erdmann条件。
基于这个条件，可以以下内容讨论了Hamilton函数法求解最优控制时，有关状态变量、协态变量、Hamilton函数的连续性问题。

3.1 $\mathbf{u}(t)$分段连续时的角点条件

控制$\mathbf{u}(t)$分段连续时，控制方程$\frac{\partial H}{\partial \mathbf{u}(t)}=0$对它分段成立，$t_i\in (0,t_f),i=1,\dots ,n$，如下图：

.若定义Hamilton函数$H\triangleq L+{\lambda }^{\mathrm{T}}f$，由角点处的Weierstrass-Erdmann条件，在每一个$t_i$处都有条件
$$\begin{array}{rl}\lambda (t_i-)& =\lambda (t_i+)\\ {H|}_{t_i-}& ={H|}_{t_i+}\end{array}\text{\hspace{1em}(4)}$$

即Hamilton函数连续，且协态变量连续。

3.2 $\mathbf{x}(t)$分段连续时的内点约束

若状态变量$x(t)$分段连续$\mathbf{x}(t_i-)=\mathbf{x}(t_i+)$，但在某些点有内点条件（Interior-Point constraints，Internal Constraints），典型案例如动力学方程是多阶段拼接而成的情况。本小节来自[2]中3.5节：
$${\psi }^{(i)}(\mathbf{x}(t_i),t_i)=0,{\psi }^{(i)}\in {\mathbb{R}}^{q_i},t_0<t_i<t_f,i=1,2,\cdots ,N$$

以上这个式子可以表达内点状态指定的条件，即 x i ( t ∗ ) = x i ∗ t 0 < t ∗ < t f , i = { 1 , 2 , … , N } x_{i}\left(t^{*}\right)=x_{i}^{*} \quad t_{0}<t^{*}<t_{f}, \quad i =\{1,2, \ldots, N\} xi​(t∗)=xi∗​t0​<t∗<tf​,i={1,2,…,N}。

此种情况下，在每一个角点处条件是：
$$\lambda (t_i-)=\lambda (t_i+)+{\mu }^{\mathrm{T}}\frac{\partial {\psi }^{(i)}}{\partial \mathbf{x}}\text{\hspace{1em}(5)}$$

以及内点时间$t_i$服从$$H(t_i+)=H(t_i-)+{\mu }^{\mathrm{T}}\frac{\partial {\psi }^{(i)}}{\partial t}\text{\hspace{1em}(6)}$$其中$\mu \in {\mathbb{R}}^{q_i}$，对应为该内点约束的Lagrange乘子。以上这个内点约束$(5,6)$对于分段连续的状态方程也成立
KaTeX parse error: Unexpected end of input in a macro argument, expected '}' at position 20: …in{aligned}\dot\̲m̲a̲t̲h̲b̲f̲ ̲x&=\left\{\begi…

简单来说就是，Hamiltonian和协态变量$\lambda (t)$在每一个内点约束附近发生间断。

3.3 $\mathbf{x}(t)$分段不连续时的内点约束

若状态变量$x(t)$是分段函数，在每一段连续可导，而每一段不连续$\mathbf{x}(t_i-)\ne \mathbf{x}(t_i+)$。典型案例如多脉冲轨道转移。如果这样的系统在某些点有内点条件（Interior-Point constraints），问题描述如[2]中3.7节的截图：

此种情况下，如上一章节所定义的标量函数，
$$\begin{gathered} H^{(i)}\triangleq L^{(i)}+{\lambda }^{\mathrm{T}}{f}^{(i)} \\ \Phi \triangleq \phi +\sum _{j=0}^N[{\mu }^{(i)}{]}^{\mathrm{T}}{\psi }^{(i)} \end{gathered}$$

间断点处的状态变量不连续，但遵循约束条件；协态变量有：
$$\begin{array}{rl}& {\lambda }^T\left(t_i-\right)=\frac{\partial \Phi }{\partial x\left(t_i-\right)};i=1,\dots ,N\\ & {\lambda }^T\left(t_i+\right)=-\frac{\partial \Phi }{\partial x\left(t_i+\right)};i=0,\dots ,N-1,\end{array}\text{\hspace{1em}(7)}$$

如果内点的到达时间未$t_i$指定，还有哈密尔顿函数服从以下$N$个式子：
$$H^{(i+1)}\left(t_{i+}\right)=\frac{\partial \Phi }{\partial t_i}+H^{(i)}\left(t_{i-}\right),i=0,\dots ,N$$

控制变量由$x(t)$和$\lambda (t)$推导得到。这个问题的实例以及其求解方法可见沈红新博士论文[4]。

3.4 例1、内点约束问题

最短时间拦截且飞行中途经过一个点的问题，如下
$$\begin{gathered} \underset{\theta (t)}{\min }J=t_f \\ \text{s.t.}\{\begin{array}{c}\dot{x}=u,\dot{y}=v\\ \dot{u}=a\cos \theta ,\dot{v}=a\sin \theta \end{array} \end{gathered}$$

已知初始点状态、落点位置、经过点的位置，即
$$\begin{gathered} \mathbf{x}(0)=[x(0),y(0),u(0),v(0){]}^{\mathrm{T}}=0, \\ N(\mathbf{x}(t_1),t_1)=\left[\begin{array}{c}x(t_1)-x_1\\ y(t_1)-y_1\end{array}\right]=0,t_1\in (0,t_f) \\ \psi (\mathbf{x}(t_f),t_f)=\left[\begin{array}{c}x(t_f)-x_f\\ y(t_f)\end{array}\right]=0 \end{gathered}$$

且$t_1$未知。求最优控制${\theta }^{\ast }(t)$

首先写出哈密尔顿函数$H=1+{\lambda }_{1}u+{\lambda }_{2}v+{\lambda }_{3}a\cos \theta +{\lambda }_{4}a\sin \theta$，协态方程
$$\dot{\lambda }(t)=-\frac{\partial H}{\partial \mathbf{x}}=\left[\begin{array}{c}0\\ 0\\ -{\lambda }_1\\ -{\lambda }_2\end{array}\right]$$

控制方程$$\begin{gathered} \frac{\partial H}{\partial \theta }=-{\lambda }_3\sin \theta +{\lambda }_{4}a\cos \theta =0 \\ \Rightarrow \tan {\theta }^{\ast }=\frac{{\lambda }_4}{{\lambda }_3} \end{gathered}$$

可见只要求出协态变量就可以得到最优控制。对终端约束引入拉格朗日乘子$\mu \in {\mathbb{R}}^2,\Phi =\varphi +{\mu }^{\mathrm{T}}\psi$，终端约束与终端时间满足：
$$\begin{gathered} \lambda (t_f)=\frac{\partial \Phi }{\partial \mathbf{x}}=[{\mu }_1,{\mu }_2,0,0{]}^{\mathrm{T}} \\ H(t_f)+\frac{\partial \Phi }{\partial t_f}=1+{\mu }_{1}u(t_f)+{\mu }_{2}v(t_f)=0 \end{gathered}$$

对内点约束引入拉格朗日乘子$\pi \in {\mathbb{R}}^2$，内点约束满足：
$$\lambda (t_{1-})=\lambda (t_{1+})+{\pi }^{\mathrm{T}}\frac{\partial N}{\partial \mathbf{x}}$$

推导可得
$$\begin{gathered} {\lambda }_{1-}={\lambda }_{1+}+{\pi }_1={\mu }_1+{\pi }_1 \\ {\lambda }_{2-}={\lambda }_{2+}+{\pi }_2={\mu }_2+{\pi }_2 \end{gathered}$$

${\lambda }_{3,4}$在$t_1$处连续，且可得$\theta (t)$连续。由于协态变量是分段的，在两个阶段都不变而${\lambda }_{1,2}$只在$t_1$处跳变，从$t_f$时刻往前推导可以得到
$$\begin{gathered} {\lambda }_1(t)=\{\begin{array}{ll}{\mu }_1+{\pi }_1& t\in [0,t_1)\\ {\mu }_1& t\in (t_1,t_f]\end{array} \\ {\lambda }_2(t)=\{\begin{array}{ll}{\mu }_2+{\pi }_2& t\in [0,t_1)\\ {\mu }_2& t\in (t_1,t_f]\end{array} \\ {\lambda }_3(t)=\{\begin{array}{ll}{\mu }_1{t}_f+t_1({\mu }_1+{\pi }_1)-(2{\mu }_1+{\pi }_1)t& t\in [0,t_1)\\ {\mu }_1(t_f-t)& t\in (t_1,t_f]\end{array} \\ {\lambda }_4(t)=\{\begin{array}{ll}{\mu }_2{t}_f+t_1({\mu }_2+{\pi }_2)-(2{\mu }_2+{\pi }_2)t& t\in [0,t_1)\\ {\mu }_2(t_f-t)& t\in (t_1,t_f]\end{array} \end{gathered}$$

另外，内点时间$H({\cdot }^{\ast },t)$不连续，即$$\begin{gathered} H({\cdot }^{\ast },t_{1-})=H({\cdot }^{\ast },t_{1+})-{\pi }^{\mathrm{T}}\frac{\partial N}{\partial t_1} \\ \Rightarrow {\lambda }_{1-}u(t_1)+{\lambda }_{2-}v(t_1)={\lambda }_{1+}u(t_1)+{\lambda }_{2+}v(t_1) \\ \Rightarrow u(t_1){\pi }_1=-v(t_1){\pi }_2 \end{gathered}$$

把以上这些条件代入解析表达式进行数值求解可得最优控制，由于计算比较复杂，后面的就忽略了。我按照直接法写了一个GPOPS程序，求解结果大概是这样，需要的同学可以在CSDN下载。

参考文献

[1] 邢继祥. 最优控制应用基础[M]. 科学出版社, 2003.
[2] Bryson A E , Ho Y C ,Applied optimal control : optimization, estimation, and control[J]. IEEE Transactions on Systems Man & Cybernetics, 1975
[3] Moritz Diehl, Numerical Optimal Control (draft), 2011
[4]沈红新. 基于解析同伦的月地应急返回轨迹优化方法[D].国防科学技术大学,2014.