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

Hamilton函数方法是变分法应用在控制系统上的标准化方法，即使不懂变分法，简单套用表格中的公式也可以列写出方程，这个方法是最优控制理论用的最多的方法。

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

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

其中的Hamilton函数 $H(x,\dot x,\lambda,t)=L(x,\dot x,t)-\lambda^{\mathrm T}f(x,\dot x,t)$。控制系统中更常见的一阶非线性系统方程，问题是这样的： $t_f$给定，终端状态未知或已知（仅边界条件不同），除状态方程外没有约束，且
$\dot{x}=f[x(t), u(t), t] ; \quad x\left(t_{o}\right)=x_0\quad t_{o} \leq t \leq t_{f}\\ \min_{u(t)}J=\varphi\left[x\left(t_{f}\right), t_{f}\right]+\int_{t_{o}}^{t_{f}} L[x(t), u(t), t] d t \tag 1$

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

对 $x(t)$、 $u(t)$和 $\lambda(t)$都考虑Euler方程，即 $\bar H_x-\frac{\text d}{\text d t}\bar H_{\dot x}=0$以及 $\bar H_u=0$， $\bar H_\lambda=0$

$\begin{aligned} \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{aligned}$

此外还有状态方程和边界条件：终端固定 $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] \tag 2$

那么性能指标化为：
$\begin{aligned} J[x(t),\dot x(t),u(t),t] &=\varphi(0)-\lambda^{\mathrm T} x\big|_0^{t_f} &+\int_{t_{o}}^{t_{f}}(H+\dot{\lambda}^{\mathrm T} x) d t \end{aligned}$

此时，Euler方程变为
$\begin{aligned} \dot{\lambda}&=-\frac{\partial H}{\partial x}=-\frac{\partial L}{\partial x}-\lambda^{\mathrm T}\frac{\partial f}{\partial x}\tag{3}\\ \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{aligned}$

这样，最优控制问题被规范化为3个Euler方程，按公式 $(3)$，依次是协态方程、状态方程和控制方程，方程按照Hamilton函数的偏导数的形式非常简洁，这样就构成了两点边值问题，需要用数值方法来求解。
按照公式 $(1)-(3)$的过程进行展开来求解，哈密尔顿函数法求解最优控制问题的具体过程如下：

• 首先写出性能指标 $J=L+\lambda^Tf$
• 依次列写协态方程 $\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(*,t_f)$
• 求解整个Hamilton系统

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

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

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

问题描述	未知变量个数	边界条件	横截条件
$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$	$\lambda(t_f)=\frac{\partial \varphi(\cdot^*,t_f)}{\partial x}$
$t_f$自由， $x_f$给定	$2n+1$	$x(t_0)=x_0,x(t_f)=x_f$	$H(\cdot^*,t_f)+\frac{\partial \varphi(\cdot^*,t_f)}{\partial t}=0$
$t_f，x_f$均自由	$2n+1$	$x(t_0)=x_0$	$\lambda(t_f)=\frac{\partial \varphi}{\partial x};\\H(\cdot^*,t_f)+\frac{\partial \varphi(\cdot^*,t_f)}{\partial t}=0$

上面，性能指标不包括Meyer型，即 $\varphi(x(t_f),t_f))\equiv0$，则横截条件中出现相应的项为0。如 $t_f，x_f$均自由时，横截条件为
$\lambda(t_f)=0,H(\cdot^*,t_f)=0$

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

设终端时刻 $t_f$自由或给定，终端状态 $x_f$自由但满足代数约束，两者之间的关系为
$\psi(x_f,t_f)=0，\psi\in\Reals^m,m\lt n\tag 5$
有m个终端约束，仍考虑表达式 $(1)$所述的性能指标。这样的终端约束可以表达以下两种关系，如：

• $x_f$的部分状态量 $x_i(t_f)=x_{if},i=1,2,\dots,m<n$给定，其他状态量自由；
• $x_f$互相之间存在代数关系，如位置和速度正交 $\mathbf r_f\cdot\mathbf v_f=0$

参考文献[2]，按照Lagrange乘数法，设一个常数向量 $\mu\in\Reals^{m}$对终端约束函数进行相乘，则性能指标变成
$\begin{aligned} J&=\left[\varphi+\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\} d t\\ &=\Phi_{t_f}+\int_{0}^{t_{f}}(H-\lambda^{\mathrm T}\dot x)dt \end{aligned}$

上式仍然定义相同的Hamilton函数 $H\triangleq L+\lambda^{\mathrm T}f$，以及一个新定义的标量函数 $\Phi(\mathbf x(t_f),t_f)\triangleq \varphi+\mu^{\mathrm T} \psi\tag\ddag$用于解决终端约束。接下来对终端时刻的性能指标求全微分：
$\begin{aligned} d J=\left(\left(\frac{\partial \Phi}{\partial t}+L\right) d t+\frac{\partial \Phi}{\partial x} d x\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) d t \end{aligned}$

并考虑 $\delta x(t)=\text d x(t)-\dot x(t)\text d t$，上式可变换为
$\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 \tag 6$

按照最优性的必要条件，令每一项的系数都为0，可以得到Euler方程
$\begin{aligned} \dot{\lambda}&=-\frac{\partial H}{\partial x}=-\frac{\partial L}{\partial x}-\lambda^{\mathrm T}\frac{\partial f}{\partial x}\\ \dot{x}&=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{aligned}\tag{7}$

边界条件和横截条件
$\begin{aligned} \lambda^{\mathrm T}\left(t_{f}\right)=\frac{\partial \Phi(\cdot^*,t_f)}{\partial x}=\frac{\partial \varphi(x_f,t_f)}{\partial x}+\mu^{\mathrm T} \frac{\partial \psi(x_f,t_f)}{\partial x} \\ \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 \end{aligned}\tag{8}$

可见文献[2]按照公式 $(6)$推导的结果和变分法得到的结果完全一致。实际上，方程 $(6)$就是把所有的边界条件写进一个式子里的表达形式。虽然推导麻烦，但是很凝练。其中 $x(t),\lambda(t)$以及Lagrange乘数 $\mu$未知，以下再给出表格总结：

问题描述	未知变量个数	边界条件	横截条件
$t_f$给定， $x_f$自由，且有终端约束 $\psi(x_f,t_f)=0$	$2n+m$	$x(t_0)=x_0\\ \psi(x_f,t_f)=0$	$\lambda(t_f)=\frac{\partial \Phi}{\partial x}\equiv\frac{\partial \varphi}{\partial x}+\mu^{\mathrm T}\frac{\partial\psi}{\partial x}$
$t_f$给定， $x_f$自由，且有终端约束 $\psi(x_f,t_f)=0$,(Lagrange型性能指标)	$2n+m$	$x(t_0)=x_0\\ \psi(x_f,t_f)=0$	$\lambda(t_f)=\mu^{\mathrm T}\frac{\partial\psi}{\partial x}$
$t_f，x_f$均自由，且有终端约束 $\psi(x_f,t_f)=0$	$2n+m+1$	$x(t_0)=x_0\\ \psi(x_f,t_f)=0$	$\lambda(t_f)=\frac{\partial \Phi}{\partial x}\equiv\frac{\partial \varphi}{\partial x}+\mu^{\mathrm T}\frac{\partial\psi}{\partial x};\\ \frac{\partial \varphi}{\partial t}+\mu^{\mathrm T} \frac{\partial \psi}{\partial t}+\left(\frac{\partial \varphi}{\partial x}+\mu^{\mathrm T} \frac{\partial \psi}{\partial x}\right) f+L=0,(t=t_{f})$
$t_f，x_f$均自由，且有终端约束 $\psi(x_f,t_f)=0$,(Lagrange型性能指标)	$2n+m+1$	$x(t_0)=x_0\\ \psi(x_f,t_f)=0$	$\lambda(t_f)=\frac{\partial \varphi}{\partial x}+\mu^{\mathrm T}\frac{\partial\psi}{\partial x};\\ \mu^{\mathrm T} [\frac{\partial \psi}{\partial t}+ \frac{\partial \psi}{\partial x} f]+L=0,(t=t_{f})$

上表中的标量函数 $\Phi(x(t_f),t_f)$公式 $(\ddag)$定义.

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$自由，二次型性能指标
$\min_{u(t)}=\frac1 2\mathbf x_f^{\mathrm T}\text Q\mathbf x_f+\frac1 2\int_0^{t_f}\text R\mathbf u^2$

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

写出Hamilton函数
$H=\frac1 2\text R u^2+\lambda_1\omega+\lambda_2\dot\omega$

协态方程
$\begin{aligned} \dot\lambda_1&=-\frac{\partial H}{\partial \theta}=\lambda_2mgl\cos\theta/I\\ \dot\lambda_2&=-\frac{\partial H}{\partial \theta}=-\lambda_1 \end{aligned}$

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

横截条件
$H(t_f)+\frac{\partial \varphi(x_f)}{\partial t}=\frac1 2\text R u^2+\lambda_2u/I=0\implies \lambda_2(t_f)=0$

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

4.2 连续推力轨道转移问题

轨道动力学方程 $\mathbf{\ddot r}=\mathbf -\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=\frac1 2\int_{t_0}^{t_f}\mathbf a^{\mathrm T}\mathbf a\text d t$。
套用Hamilton函数法，状态变量
$\mathbf x=\begin{bmatrix}\mathbf r\\ \mathbf v\end{bmatrix}\\ \mathbf f(\mathbf x)=\begin{bmatrix}\mathbf r\\ -\frac\mu{r^3}\mathbf r+\mathbf a\end{bmatrix}$

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

协态方程
$\begin{aligned} \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{aligned}$

对终端约束引入Lagrange乘数 $\mu\in\Reals^1$，查表得到横截条件
$\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$

最优控制
$\frac{\partial{H}}{\partial \mathbf a}=\mathbf a+\lambda_v=0\tag 9$
则最优控制的控制律为 $\mathbf a=-\lambda_{\mathbf v}$，公式 $(9)$最早由Lawden提出，被称为主矢量理论。代入控制，有12个未知变量，
$\begin{aligned} \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{aligned}$

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