最优控制理论 二、哈密尔顿函数法

Hamilton函数方法是变分法应用在控制系统上的标准化方法，即使不懂变分法，简单套用表格中的公式也可以列写出方程，这个方法是最优控制理论用的最多的方法。
本篇博客是本系列最长也是最重要的一篇，持续更新，欢迎同学和朋友们提出修改建议。

1. 规范化的最优控制问题

按照第一章最优控制理论 一、变分法和泛函极值问题，我们已经讨论了有动力学方程约束$\mathbf{f}(\mathbf{x},\dot{\mathbf{x}},t)=0$的动态系统，若无其他约束，这个系统的最优轨线遵循以下必要条件
$$\begin{array}{r}{H}_x-\frac{\text{d}}{\text{d}t}{H}_{\dot{x}}=0\\ \mathbf{f}(\mathbf{x}(t),\dot{\mathbf{x}(t)},t)=0\end{array}$$

其中的Hamilton函数$H(x,\dot{x},\lambda ,t)=L(x,\dot{x},t)-{\lambda }^{\mathrm{T}}f(x,\dot{x},t)$。控制系统中更常见的一阶非线性系统方程，问题是这样的：$t_f$给定，终端状态未知或已知（仅边界条件不同），除状态方程外没有约束，且
$$\begin{gathered} \dot{x}=f[x(t),u(t),t];x\left(t_o\right)=x_0{t}_o\le t\le t_f \\ \underset{u(t)}{\min }J=\phi \left[x\left(t_f\right),t_f\right]+{\int }_{t_o}^{t_f}L[x(t),u(t),t]dt\text{\hspace{1em}(1)} \end{gathered}$$

式中包括了控制项$u(t)$。这样的问题仍按照上一章的方法来考虑，对动力学方程约束引入Lagrange乘子
$$\begin{gathered} J[x(t),\dot{x}(t),u(t),t]=\phi (0)+{\int }_t^{t_f}\left\{L+{\lambda }^{\mathrm{T}}[f(x,u,t)-\dot{x}]+\frac{\text{d}\phi (x,t)}{\text{d}t}\right\}dt \\ =\phi (0)+{\int }_t^{t_f}\bar{H}(x,\dot{x},\lambda ,u,t)\text{d}t\text{\hspace{1em}(†)} \end{gathered}$$

对$x(t)$、$u(t)$和$\lambda (t)$都考虑最优性必要条件，即${\bar{H}}_x-\frac{\text{d}}{\text{d}t}{\bar{H}}_{\dot{x}}=0$以及${\bar{H}}_u=0$，${\bar{H}}_{\lambda }=0$，可以得到Euler方程：

$$\begin{array}{c}\begin{array}{r}\frac{\partial L}{\partial x}+\frac{\partial f^{\mathrm{T}}}{\partial x}\lambda (t)+\dot{\lambda }(t)=0\\ f(x,u,t)-\dot{x}=0\\ \frac{\partial L}{\partial u}+\frac{\partial f^{\mathrm{T}}}{\partial u^{\lambda }}\lambda (t)=0\end{array}\end{array}\text{\hspace{1em}(EL1)}$$

此外还有状态方程和边界条件：终端固定$x(t_f)=x_f$或终端自由${\bar{H}}_{\dot{x}}(t_f)=0$.

2. Hamilton函数法

上面这个方程的形式不是很好，我们重新定义一个哈密尔顿函数：

$$H[x(t),u(t),\lambda (t),t]\triangleq L[x(t),u(t),t]+{\lambda }^{\mathrm{T}}(t)f[x(t),u(t),t]\text{\hspace{1em}(2)}$$

那么性能指标化为：
$$\begin{array}{rlr}J[x(t),\dot{x}(t),u(t),t]& =\phi (0)-{\lambda }^{\mathrm{T}}x{|}_0^{t_f}& +{\int }_{t_o}^{t_f}(H+{\dot{\lambda }}^{\mathrm{T}}x)dt\end{array}$$

此时，仿照上面式$\text{(EL1)}$，可以重写Euler方程为以下几个等式：
$$\begin{array}{rl}\dot{\lambda }& =-\frac{\partial H}{\partial x}=-\frac{\partial L}{\partial x}-{\lambda }^{\mathrm{T}}\frac{\partial f}{\partial x}\\ \dot{x}& =\frac{\partial H}{\partial \lambda }=f(x,u,t)\\ 0& =\frac{\partial H}{\partial u}=\frac{\partial L}{\partial u}+{\left(\frac{\partial f}{\partial u}\right)}^{\mathrm{T}}\lambda \end{array}\text{\hspace{1em}(3,EL2)}$$

这样，最优控制问题被规范化为3个Euler方程，按公式$(3)$，依次是协态方程、状态方程和控制方程，方程按照Hamilton函数的偏导数的形式非常简洁。由于式$\text{(EL2)}$由2个微分方程构成，实际上Hamilton函数法构造了两点边值问题，只要同时得到协态、状态，就相当于获得了最优控制。

按照公式$(1)-(3)$的过程进行展开来求解，哈密尔顿函数法求解最优控制问题的具体过程如下：

• 首先写出哈密尔顿函数 $H=L+{\lambda }^{T}f$
• 依次列写协态方程 $\dot{\lambda }=-\frac{\partial H}{\partial x}$、控制方程 $\frac{\partial H}{\partial u}=0$
• 将最优控制代入状态方程 $\dot{x}=f(x,u,t)$
• 写出边界条件和横截条件如 $x(t_f),\lambda (t_f),H(\ast ,t_f)$
• 求解整个Hamilton系统

这个方法在通用性很强，可以解决大多数无约束问题、以及带有终端约束的最优控制问题。

2.1 Hamilton函数的性质

2.1.1 哈密尔顿系统

哈密尔顿函数的引入，使得无约束最优控制的Euler-Lagrange方程可以用一阶常微分方程组描述。两者之间的关系如下，一阶必要条件Euler-Lagrange为：
$$\frac{\partial L[x(t),\dot{x}(t),t]}{\partial x}-\frac{\text{d}}{\text{d}t}\frac{\partial L[x(t),\dot{x}(t),t]}{\partial \dot{x}}=0$$

定义协态变量$$\lambda (t)=-\frac{\partial L[x(t),\dot{x}(t),t]}{\partial \dot{x}}$$把E-L方程代入得到$\dot{\lambda }=-L_x$. 定义哈密尔顿函数$$H[x(t),\lambda (t),t]=L[x(t),\dot{x}(t),t]+{\lambda }^{\mathrm{T}}(t)f[x(t),u(t),t]$$，并求它的梯度
$$\begin{array}{rl}\frac{\partial H}{\partial x}=& {\lambda }^{\mathrm{T}}\frac{\partial f}{\partial x}+\frac{\partial L}{\partial x}+\frac{\partial L}{\partial \dot{x}}\frac{\partial \dot{x}}{\partial x}=\frac{\partial L}{\partial x}=-\dot{\lambda }\end{array}$$

另外，$$\dot{x}=f=\frac{\partial H}{\partial \lambda }$$，于是得到以下Hamilton系统
$$\begin{array}{rl}\dot{x}=& \frac{\partial H}{\partial \lambda }\\ \dot{\lambda }=& -\frac{\partial H}{\partial x}\end{array}$$

以上这个结论在求解BVP问题时会用到，它的性质对于数值求解的影响是负面性的，Diehl【3】有以下描述：

2.1.2 时间不变性

沿最优轨线$x^{\ast }(t)$，Hamilton函数对时间的全导数等于其对时间的偏导数，即
$$\frac{\mathrm{d}H}{\mathrm{d}t}=\frac{\partial H}{\partial t}\text{\hspace{1em}(1)}$$

证明：对Hamiltonian按照链式求导法则全导数：
$$\frac{\mathrm{d}H}{\mathrm{d}t}=\frac{\partial H^{\mathrm{T}}}{\partial x}\dot{x}+\frac{\partial H^{\mathrm{T}}}{\partial \lambda }\dot{\lambda }+\frac{\partial H^{\mathrm{T}}}{\partial u}\dot{U}+\frac{\partial H}{\partial t}$$

考虑到最优轨线附近满足
$$\begin{array}{rl}-\frac{\partial H}{\partial x}& =\dot{\lambda }\\ \frac{\partial H}{\partial u}& =0\\ \frac{\partial H}{\partial \lambda }& =f=\dot{x}\end{array}$$

代入则公式$(4)$可证。$\square$
此外，若Hamiltonian不显含时间t，则显然有
$$\frac{\mathrm{d}H}{\mathrm{d}t}=\frac{\partial H}{\partial t}=0$$

于是可得$H(x^{\ast }(t),u^{\ast }(t),{\lambda }^{\ast }(t),t)=\text{Const}$，若进一步考虑边界条件，对于$t_f$固定的，则$H(\ast ,t)=H(0)$； 对于$t_f$自由的问题，查表，有终端约束$H(\ast ,t_f)=0$，则$H(\ast ,t)=0$，也就是说终端时间自由的最优控制必然有哈密尔顿函数始终为0.

2.2 Hamilton函数的边界条件和横截条件

除了Euler方程，还要考虑边界条件和定解条件才能实际求解。
方程中$x(t),\lambda (t)\in {\mathbb{R}}^n,u(t)\in {\mathbb{R}}^m$总共有$2n+m$个未知的时变参数。协态方程和状态方程$x(t),\lambda (t)$是一阶常微分方程组，需要知道$2n$个边界条件才能求解；控制方程$u(t)$是代数方程，由$x(t)$和$\lambda (t)$直接得到。
下面给出几种常用的边界条件和横截条件，式中变量均按照问题$(1)$的表述。

表1. 不含终端约束时的定解条件

问题描述	未知变量个数	边界条件	横截条件
$t_f,x_f$均给定	$2n$	$x(t_0)=x_0,x(t_f)=x_f$	\
$t_f$给定，$x_f$自由	$2n$	$x(t_0)=x_0$	$\begin{array}{r}\lambda (t_f)=\frac{\partial \phi ({\cdot }^{\ast },t_f)}{\partial x}\end{array}$
$t_f$自由，$x_f$给定	$2n+1$	$x(t_0)=x_0,x(t_f)=x_f$	$\begin{array}{r}H({\cdot }^{\ast },t_f)+\frac{\partial \phi ({\cdot }^{\ast },t_f)}{\partial t}=0\end{array}$
$t_f，x_f$均自由	$2n+1$	$x(t_0)=x_0$	$\begin{array}{rl}& \lambda (t_f)=\frac{\partial \phi }{\partial x};\\ & H({\cdot }^{\ast },t_f)+\frac{\partial \phi ({\cdot }^{\ast },t_f)}{\partial t}=0\end{array}$

• 上面问题$(1)$中的性能指标若为Meyer型，即$\phi (x(t_f),t_f))\equiv 0$，则横截条件中出现相应的项为0。
• 另外，表1中的形式是简写，还要把哈密尔顿函数展开。如对于第四行$t_f,x_f$均自由时，可以把横截条件代入Hamiltonian，得
  $$\begin{array}{rl}& \lambda (t_f)=\frac{\partial \phi }{\partial x}\\ & H({\cdot }^{\ast },t_f)+\frac{\partial \phi ({\cdot }^{\ast },t_f)}{\partial t}={\left[L+\frac{\partial \phi }{\partial x}f+\frac{\partial \phi }{\partial t}\right]}_{t_f}=0\end{array}$$

3. 终端约束时的横截条件

设终端时刻$t_f$自由或给定，终端状态$x_f$自由但满足代数约束，两者之间的关系为
$$\psi (x(t_f),t_f)=0，\psi \in {\mathbb{R}}^q,q<n\text{\hspace{1em}(5)}$$

有q个终端约束，仍考虑表达式$(1)$所述的性能指标。这样的终端约束可以表达以下两种关系，如：

• $x_f$的部分状态量$x_i(t_f)=x_f^{(i)},i=1,2,\dots ,q<n$给定，其他状态量自由；
• $x_f$互相之间存在代数等式约束关系；

参考文献[2]，按照Lagrange乘数法，设一个常数向量$\mu \in {\mathbb{R}}^q$对终端约束函数进行相乘，则性能指标变成
$$\begin{array}{rl}J& ={\left[\phi +{\mu }^{\mathrm{T}}\psi \right]}_{t_f}+{\int }_0^{t_f}\left\{L(x,u,t)+{\lambda }^{\mathrm{T}}[f(x,u,t)-\dot{x}]\right\}dt\\ & ={\Phi }_{t_f}+{\int }_0^{t_f}(H-{\lambda }^{\mathrm{T}}\dot{x})dt\end{array}$$

上式仍然定义相同的Hamilton函数$H\triangleq L+{\lambda }^{\mathrm{T}}f$，以及一个新定义的标量函数$$\Phi (\mathbf{x}(t_f),t_f)\triangleq \phi +{\mu }^{\mathrm{T}}\psi \text{\hspace{1em}(‡)}$$用于解决终端约束。接下来对终端时刻的性能指标求全微分：
$$\begin{array}{rl}dJ={\left(\left(\frac{\partial \Phi }{\partial t}+L\right)dt+\frac{\partial \Phi }{\partial x}dx\right)}_{t_f}& +{\int }_0^{t_f}\left(\frac{\partial H}{\partial x}\delta x+\frac{\partial H}{\partial u}\delta u-{\lambda }^{\mathrm{T}}\delta \dot{x}\right)dt\end{array}$$

并考虑$\delta x(t)=\text{d}x(t)-\dot{x}(t)\text{d}t$，上式可变换为
$$\begin{gathered} \text{d}J={\left(\frac{\partial \Phi }{\partial t}+L+{\lambda }^{\mathrm{T}}\dot{x}\right)}_{t_f}\text{d}{t}_f+{\left[\left(\frac{\partial \Phi }{\partial x}-{\lambda }^{\mathrm{T}}\right)\text{d}x\right]}_{t_f}+ \\ {\left({\lambda }^{\mathrm{T}}\delta x\right)}_{t_0}+{\int }_0^{t_f}\left[\left(\frac{\partial H}{\partial x}+{\dot{\lambda }}^{\mathrm{T}}\right)\delta x+\frac{\partial H}{\partial u}\delta u\right]\text{d}t\text{\hspace{1em}(6)} \end{gathered}$$
针对上公式的结果进行分析。

3.1 性能指标变分的推导结果

公式$(6)$中，按照最优性的必要条件，令每一项的系数都为0，可以得到Euler方程、
$$\begin{array}{rl}\dot{\lambda }& =-\frac{\partial H}{\partial x}=-\frac{\partial L}{\partial x}-{\lambda }^{\mathrm{T}}\frac{\partial f}{\partial x}\\ \frac{\partial H}{\partial u}& =0=\frac{\partial L}{\partial u}+{\left(\frac{\partial f}{\partial u}\right)}^{\mathrm{T}}\lambda \end{array}\text{\hspace{1em}(7)}$$

协态变量的定解条件、
$$\begin{array}{r}{\lambda }^{\mathrm{T}}\left(t_f\right)=\frac{\partial \Phi ({\cdot }^{\ast },t_f)}{\partial x}=\frac{\partial \phi (x_f,t_f)}{\partial x}+{\mu }^{\mathrm{T}}\frac{\partial \psi (x_f,t_f)}{\partial x}\end{array}\text{\hspace{1em}(8)}$$

以及时间$t_f$不固定的定解条件：
$${\left(\frac{\partial \Phi }{\partial t}+{\lambda }^{\mathrm{T}}\dot{x}+L\right)}_{t=t_f}\equiv {\left(\frac{\text{d}\Phi }{\text{d}t}+L\right)}_{t=t_f}=0\text{\hspace{1em}(9)}$$

可见文献[2]按照公式$(6)$推导的结果和变分法得到的结果完全一致。

3.2 初值部分未知的处理

公式$(6)$中，如果初始状态完全给定，即$\delta \mathbf{x}(t_0)=0$，则总和${\lambda }^{\mathrm{T}}\delta \mathbf{x}{|}_{t_0}=0$；如果初始状态部分给定，而其余状态未知，即$x_j(t_0),j=1,2,...,k$，也就是任意的变分需要遵循：
$$\delta x_j(t_0)\{\begin{array}{cc}\ne 0& ,j=1,2,...,k\\ =0& ,j=k+1,...,n\end{array}$$

要使得${\lambda }^{\mathrm{T}}\delta \mathbf{x}{|}_{t_0}=0$，则要求对应拉格朗日乘子为0，即${\lambda }_j(t_0)=0,j=1,2,...,k$。于是有这样的结论：初值未给定状态的协态变量初值为0，即$${\lambda }_j(t_0)=0,若x_j(t_0)未知,j=1,2,...k。\text{\hspace{1em}(10)}$$在BVP问题中，$x_j(t_0)$未知而${\lambda }_j(t_0)$已知，构成的问题仍然可解。

3.3 表格总结

实际上，公式$(6)$就是把所有的边界条件写进一个式子里的表达形式，虽然推导麻烦，但是很凝练。其中$x(t),\lambda (t)$以及Lagrange乘数$\mu$未知，以下再给出表格总结：

表2. 不同问题的定解条件

问题描述	未知变量个数	边界条件	横截条件
$t_f,x_f$均给定	$2n$	$x(t_0)=x_0;x(t_f)=x_f$	\
$t_f$给定，$x_f$自由	$2n\to (x,\lambda )$	$x(t_0)=x_0$	$\begin{array}{r}\lambda (t_f)=\frac{\partial \phi }{\partial x}\end{array}$
$t_f,x_f$均自由	$2n+1\to (x,\lambda ,t_f)$	$x(t_0)=x_0$	$\begin{array}{rl}& \lambda (t_f)=\frac{\partial \phi }{\partial x};\\ & {\left\{L+\frac{\partial \phi }{\partial x}f+\frac{\partial \phi }{\partial t}\right\}}_{t_f}=0\end{array}$
$t_f$给定，$x_f$自由，且有终端约束$\psi (x_f,t_f)=0$	$2n+q\to (x,\lambda ,\mu )$	$$\begin{gathered} x(t_0)=x_0; \\ \psi (x_f,t_f)=0 \end{gathered}$$	$\begin{array}{r}\lambda (t_f)=\frac{\partial \Phi }{\partial x}\end{array}$
$t_f，x_f$均自由，且有终端约束$\psi (x_f,t_f)=0$	$2n+q+1\to (x,\lambda ,\mu ,t_f)$	$$\begin{gathered} x(t_0)=x_0; \\ \psi (x_f,t_f)=0 \end{gathered}$$	$\begin{array}{rl}& \lambda (t_f)=\frac{\partial \Phi }{\partial x};\\ & {\left\{L+\frac{\partial \Phi }{\partial x}f+\frac{\partial \Phi }{\partial t}\right\}}_{t_f}=0\end{array}$

1）. 第4、5行说明

上表第4、5行因为引入了拉格朗日乘子$\mu$而与第2、3行形式一致，显得简洁，其中的标量函数$\Phi (x(t_f),t_f)$由公式$(‡)$定义。终端状态未知所产生的横截条件展开为：
$$\lambda (t_f)=\frac{\partial \Phi }{\partial x}{|}_{t=t_f}\equiv \frac{\partial \phi }{\partial x}+{\mu }^{\mathrm{T}}\frac{\partial \psi }{\partial x}\text{\hspace{1em}(11)}$$

第5行比第4行多了一个条件，这个条件用于确定自由的$t_f$。定解条件展开后为：
$${\left[L+\left(\frac{\partial \phi }{\partial t}+{\mu }^{\mathrm{T}}\frac{\partial \psi }{\partial t}\right)+\left(\frac{\partial \phi }{\partial x}+{\mu }^{\mathrm{T}}\frac{\partial \psi }{\partial x}\right)f\right]}_{t_f}=0\text{\hspace{1em}(12)}$$

事实上，上式$(12)$可以这样记忆。
$$\begin{array}{rl}& H^{\ast }({\cdot }^{\ast },t_f)+\frac{\partial \Phi ({\cdot }^{\ast },t_f)}{\partial t}=0\\ & \Phi (\mathbf{x}(t_f),t_f)\equiv \phi +{\mu }^{\mathrm{T}}\psi \\ & H^{\ast }[x(t),\lambda (t),x(t_f),\lambda (t_f),t,t_f]\equiv L+{\lambda }^{\mathrm{T}}f+{\mu }^{\mathrm{T}}\psi \end{array}$$

2）. 其他型性能指标

表2中的内容假设性能指标为Bolza型（即包含终端项与积分项），但是如果性能指标只包含积分项（Lagrange型）或终端函数项（Meyer型），只需要把不存在的函数项设为0即可。

3）. 终端状态部分给定的处理

另外，作为终端约束$(5)$的一种特殊形式，如果部分终端状态已知，即
$$\psi [x(t_f),t_f]=x_i(t_f)-x_{if}=0，i=1,2,...,q,q<n$$

这种情况仍然可以套用表格中的边界条件与横截条件
$$\begin{array}{rl}{x}_i(t_f)& =x_{if}\\ \lambda (t_f)& =\{\begin{array}{cc}0+{\mu }_i& ,i=1,2,...q\\ \frac{\partial \phi }{\partial x_i}& ,i=q+1,...,n\end{array}\end{array}$$

4. 应用举例

4.1 倒立摆问题

倒立摆按照方程$I\ddot{\theta }+b\dot{\theta }-mgl\sin \theta =u$,初始状态$x_0=[\theta ,\omega {]}^{\mathrm{T}}=[\pi ,0]$，控制目标$[{\theta }_f,{\omega }_f{]}^{\mathrm{T}}=[0,0]$，终端时刻$t_f$自由，二次型性能指标
$$\underset{u(t)}{\min }=\frac{1}{2}{\mathbf{x}}_f^{\mathrm{T}}\text{Q}{\mathbf{x}}_f+\frac{1}{2}{\int }_0^{t_f}\text{R}{\mathbf{u}}^2$$

首先写出一阶非线性微分方程组
$$\left[\begin{array}{c}\dot{\theta }\\ \dot{\omega }\end{array}\right]=\mathbf{f}(\mathbf{x})=\left[\begin{array}{c}\omega \\ 1/I(-mgl\sin \theta +b\omega +u)\end{array}\right]$$

写出Hamilton函数
$$H=\frac{1}{2}\text{R}{u}^2+{\lambda }_1\omega +{\lambda }_2\dot{\omega }$$

协态方程
$$\begin{array}{rl}{\dot{\lambda }}_1& =-\frac{\partial H}{\partial \theta }={\lambda }_{2}mgl\cos \theta /I\\ {\dot{\lambda }}_2& =-\frac{\partial H}{\partial \theta }=-{\lambda }_1\end{array}$$

最优控制
$$\frac{\partial H}{\partial u}=Ru+{\lambda }_2/I=0⟹u=-{\lambda }_2/RI$$

横截条件
$$H(t_f)+\frac{\partial \phi (x_f)}{\partial t}=\frac{1}{2}\text{R}{u}^2+{\lambda }_{2}u/I=0⟹{\lambda }_2(t_f)=0$$

代入数据，调用MATLAB中的$\text{sol = bvp4c(odefun,bcfun,solinit,options)}$求解这个问题，得到结果。

4.2 连续推力轨道转移问题

轨道动力学方程$\ddot{\mathbf{r}}=-\frac{\mu }{r^3}\mathbf{r}+\mathbf{a}$，初始状态已知$r(t_0)=r_0,v(t_0)=v_0$，终端时刻$t_f$给定，终端状态约束$\psi (\mathbf{r}(t_f),\mathbf{v}(t_f))={\mathbf{r}}^{\mathrm{T}}\mathbf{v}=0$。最小能量问题${\min }_{a(t)}J=\frac{1}{2}{\int }_{t_0}^{t_f}{\mathbf{a}}^{\mathrm{T}}\mathbf{a}\text{d}t$。
套用Hamilton函数法，状态变量
$$\begin{gathered} \mathbf{x}=\left[\begin{array}{c}\mathbf{r}\\ \mathbf{v}\end{array}\right] \\ \mathbf{f}(\mathbf{x})=\left[\begin{array}{c}\mathbf{r}\\ -\frac{\mu }{r^3}\mathbf{r}+\mathbf{a}\end{array}\right] \end{gathered}$$

则Hamilton函数为
$$H=\frac{1}{2}{\mathbf{a}}^{\mathrm{T}}\mathbf{a}+{\lambda }_r^{\mathrm{T}}\mathbf{v}+{\lambda }_v^{\mathrm{T}}(\mathbf{g}(\mathbf{r})+\mathbf{a})$$

协态方程
$$\begin{array}{rl}{\dot{\lambda }}_r^{\mathrm{T}}& =-\frac{\partial H}{\partial \mathbf{r}}=-{\lambda }_{\mathrm{v}}^{\mathrm{T}}\frac{\partial \mathbf{g}(\mathbf{r})}{\partial \mathbf{r}}\\ {\dot{\lambda }}_{\mathrm{v}}^{\mathrm{T}}& =-\frac{\partial H}{\partial \mathbf{v}}=-{\lambda }_r^{\mathrm{T}}\end{array}$$

对终端约束引入Lagrange乘数$\mu \in {\mathbb{R}}^1$，查表得到横截条件
$$\begin{gathered} {\lambda }_r\left(t_f\right)={\mu }^{\mathrm{T}}\frac{\partial \psi }{\partial \mathbf{r}\left(t_f\right)}=\mu {\mathbf{v}}_f \\ {\lambda }_{\mathrm{v}}\left(t_f\right)={\mu }^{\mathrm{T}}\frac{\partial \psi }{\partial \mathbf{v}\left(t_f\right)}=\mu {\mathbf{r}}_f \end{gathered}$$

最优控制
$$\frac{\partial H}{\partial \mathbf{a}}=\mathbf{a}+{\lambda }_v=0\text{\hspace{1em}(13)}$$
则最优控制的控制律为$\mathbf{a}=-{\lambda }_{\mathbf{v}}$，公式$(13)$最早由Lawden提出，被称为主矢量理论，它的主要含义是：最优推力方向总是与协态变量${\lambda }_{\mathbf{v}}$反向，只要求出协态变量就可以确定最优推力。代入最优控制的结果，构成以下12个未知变量的哈密尔顿系统，
$$\begin{array}{rl}\dot{\mathbf{r}}& =\mathbf{r}\\ \dot{\mathbf{v}}& =-\frac{\mu }{r^3}\mathbf{r}-{\lambda }_v\\ {\dot{\lambda }}_r^{\mathrm{T}}& =-{\lambda }_{\mathrm{v}}^{\mathrm{T}}\frac{\partial \mathbf{g}(\mathbf{r})}{\partial \mathbf{r}}\\ {\dot{\lambda }}_{\mathrm{v}}^{\mathrm{T}}& =-{\lambda }_r^{\mathrm{T}}\end{array}$$

1个未知常数$\mu$，边界条件共有13个，可以通过求解两点边值问题求解最优轨迹。这个问题的主要难点是确定协态变量初值，只要得到它就可以用数值积分方法求解，然而难点在于如何满足终端约束。

参考文献

[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] 还有一些不重要的内容被我放到另一篇博客里了： 最优控制理论 二+、哈密尔顿函数法的补充