【最优控制】最优控制的数值解法
最优控制里最著名的理论是 Pontryagin 极小值原理。但Pontryagin 极小值原理只是告诉我们最优控制率应该符合什么样的条件，对于复杂的问题并没有告诉我们如何找到最优的控制率，除非问题足够简单，可以通过Pontryagin 极小值原理直接解方程得到。现在，本文来介绍最优控制问题的数值解法。最优控制的数值解法本质上是将最优控制问题转化为非线性优化问题(将微分方程离散化为代数方程)，然后利用优化方法去解这个非线性优化问题。
这一节中，我们考虑求解如下最优控制问题：
$$ \begin{align} \min_u \quad & J(u) = h(x(t_f), t_f) + \int_{t_0}^{t_f} g(x(t), u(t), t) \mathrm{d} t. \\ &\dot{x}(t) = f(x(t), u(t), t), \quad t\in [t_0, t_f], \\ &x(t_0) = x_0. \end{align} \tag{1} $$
第一步：将状态方程时间离散化
为了将上述问题转化为非线性优化问题，我们需要对时间进行离散化。我们首先对状态方程进行离散化，根据状态的微分方程很容易得到：
$$ \begin{align} x(t_{k+1}) &= x(t_k) + \int_{t_k}^{t_{k+1}} f(x(t), u(t), t) \, \mathrm{d} t \\ \end{align} \tag{2} $$
如果我们用一个矩形面积来近似上式中的积分项，就可得到一个差分方程，如下所示：
$$ \begin{align} x(t_{k+1}) &= x(t_k) + f(x(t_k), u(t_k), t_k) \cdot (t_{k+1} - t_k) \\ \end{align} \tag{3} $$
简记为：
$$ \begin{align} x_{k+1} &= x_k + f(x_k, u_k, t_k) \cdot h \end{align} \tag{4} $$
这种离散化方法也被称之为一阶显示欧拉法。上式完全可以看成：原函数在 $x_k$ 处的一阶泰勒展开。如果我们用梯形（上底+下底的和乘以高除以2）来近似公式 (4) 中的积分项，就可以得到：
$$ \begin{align} x(t_{k+1}) &= x(t_k) + \frac{t_{k+1} - t_k}{2} [f(x(t_{k+1}), u(t_{k+1}), t_{k+1}) + f(x(t_k), u(t_k), t_k)] \\ \end{align} \tag{5} $$
简写为：
$$ \begin{align} x_{k+1} &= x_k + \frac{h}{2} [f(x_{k+1}, u_{k+1}, t_{k+1}) + f(x_k, u_k, t_k)] \end{align} \tag{6} $$
注意到，右边式子中又出现了一次 $x_{k+1}$，我们用公式 (6) 对它进行一次近似，那么上式可以简记为：
$$ \begin{align} K_1 &= f(x_k, u_k, t_k) \\ K_2 &= f(x_k + h \cdot K_1, u_{k+1}, t_{k} + h)\\ x_{k+1} &= x_k + \frac{h}{2} [K_1 + K_2] \end{align} \tag{7} $$
上式所表示的离散化方法也被称之为：改进欧拉法。上述方法实现简单，但积分时误差较大，现在常用的算法为四阶龙格库塔法，下面直接给出四阶龙格库塔法的形式：
$$ \begin{align} K_1 &= f(x_k, u_k, t_k) \\ K_2 &= f\left(x_k + \frac{h}{2} \cdot K_1, u_{k+1/2}, t_{k} + \frac{h}{2} \right) \\ K_3 &= f\left(x_k + \frac{h}{2} \cdot K_2, u_{k+1/2}, t_{k} + \frac{h}{2} \right) \\ K_4 &= f\left(x_k + h \cdot K_3, u_{k+1}, t_{k} + h \right) \\ x_{k+1} &= x_k + \frac{h}{6} [K_1 + 2K_2 + 2K_3 + K_4] \end{align} \tag{8} $$
在实际的实现中，四阶龙格库塔法较为常见。总之，状态方程离散化的主要目标是计算公式（4）右边的积分项，这部分可参考数值分析中【常微分方程初值问题的数值解法】相关的内容。另外，如何对时间离散化也是比较重要的（最简单的形式是均匀采样，但这样不一定好）。一般来说，更加普适的状态方程离散化可以表示为：先利用插值方法将离散的 $x_k, u_k$ 表示为连续的与 $x_k, u_k$ 有关的函数 $\hat{f}(x_k, u_k, t_k)$，用来表示状态轨迹；随后，利用状态方程构造约束，得到 $\hat{f}(x_k, u_k, t_k)$ 之间的关系。也就是先插值后配点，这部分可以参考 直接配点法（Direct Collocation） 和 Legendre伪谱法/勒让德伪谱法。
第二步：优化目标时间离散化
由于我们已经把状态和动作给时间离散化了，那么我们也得把优化目标给时间离散化，要保证能够利用离散后的变量 $x_i,u_i$ 去表示性能指标，最终得到 $\hat{J}(x_i, u_i)$。需要注意的是：$\hat{J}(x_i, u_i)$ 必须是可导的。这样就能保证优化算法能够基于梯度对变量 $u_i$ 进行优化。对于优化目标中的积分项，采用现有的数值积分方法就能保证这一点。
第三步：得到时间离散后的非线性优化问题
我们通过时间离散化方法把最优控制问题就转化为了带约束的非线性优化问题（状态方程离散化+优化目标离散化）。具体的转化一般有两种形式，打靶形式（shooting 法）或是配点形式 （collocation 法）。前者只优化动作变量 $u_i$，状态变量通过对状态方程进行 rollout 得到。后者，同时优化状态变量 $x_i$ 和动作变量 $u_i$。这里先简单介绍这两种形式的区别，文章后面通过一个案例就很容易理解两种形式的不同。
shooting 法
在 shooting 法中，$x_k$ 是通过状态转移体现在约束中。
$$ \begin{align} \min_{u_k, k = 0,…,T } \quad & J(u_0, u_1, …, u_T) \\ &x_{k+1} = x_k + f(x_k, u_k, t_k) \cdot h, \quad k = 0,…,T \\ &x_0 = c \end{align} \tag{9} $$
这里我们采用的是一阶欧拉离散化的方法，初始时刻状态 $x_0$ 已知为 $c$。可以保证任意的初始解都满足状态方程的约束，这是因为 $x_1, x_2, …$ 都是通过状态方程 rollout 得到的。这里只对 $u_k$ 进行优化。
collocation 法
在 collocation 法中，非线性优化算法需要同时对 $x_k， u_k$ 进行优化。
$$ \begin{align} \min_{x_k, u_k, k = 0,…,T} \quad & J(u_0, u_1, …, u_T, x_1, x_2, …, x_T) \\ &x_{k+1} = x_k + f(x_k, u_k, t_k) \cdot h, \quad k = 0,…,T \\ &x_0 = c \end{align} \tag{10} $$
配点法中不能保证初始解满足状态方程约束。这里不太能体现 shooting 和 collocation 之间的区别，下面会举两个例子进行详细的说明。
一个简单的例子
考虑如下最优控制问题：
$$ \begin{align} \min_{u(t)} \quad &J(u(t)) = \int_0^3 u^2(t) \mathrm{d} t, \\ \textrm{s.t.} \quad & \dot{x}(t) = v(t), \\ & \dot{v}(t) = u(t), \\ & x(0) = -2, \\ & v(0) = 1, \\ & x(3) = 0, \\ & v(3) = 0, \\ \end{align} \tag{11} $$
其含义为：在一个一维空间中，一个小球 0 时刻在 -2 这个位置，速度为 1 m/s。我们要给他加速度，保证在 3 时刻这个小球在 0 这个位置，并且速度为 0 m/s。控制过程中，所消耗的能量越低越好。
使用 shootting 法求解
为了简单起见，我们把时间离散化为 4 个时刻：0,1,2,3。时间间隔 $h = 1$。这样算出来的结果不准，主要是展示求解的过程。对于目标函数中的积分，我们采用梯形来计算每个时间微元的能量消耗。
$$ \begin{align} \int_0^3 u^2(t) \mathrm{d} t &\approx \frac{u^2_0+ u^2_1}{2} h +\frac{u^2_1+ u^2_2}{2} h + \frac{u^2_2+ u^2_3}{2} h \\ &= \frac{1}{2}u^2_0 + u^2_1 + u^2_2 + \frac{1}{2} u^2_3 \end{align} \tag{12} $$
而状态方程时间离散化后，可以表示为（一阶欧拉法）：
$$ \begin{align} & x_{k+1} = x_k + v_k \cdot h = x_k + v_k, \\ & v_{k+1} = v_k + u_k \cdot h = v_k + u_k, \\ \end{align} \tag{13} $$
那么经过我们不停的 rollout 可以得到：
$$ \begin{align} & x_0= -2 \\ & v_0= 1, \\ & x_{1} = x_0 + v_0 = -2 + 1 = -1, \\ & v_1 = v_0 + u_0 = 1 + u_0\\ & x_{2} = x_1 + v_1 = -1 + 1 + u_0 = u_0 \\ & v_2 = v_1 + u_1 = 1 + u_0 + u_1 \\ & x_{3} = x_2 + v_2 = u_0 + 1 + u_0 + u_1 = 1 + 2u_0 + u_1 = 0, \\ & v_3 = v_2 + u_2 = 1 + u_0 + u_1 + u_2 = 0,\\ \end{align} \tag{14} $$
那么最终的优化问题为：
$$ \begin{align} \min_{u_0, u_1, u_2, u_3} &\frac{1}{2}u^2_0 + u^2_1 + u^2_2 + \frac{1}{2} u^2_3 \\ \textrm{s.t.} \quad & 1 + 2u_0 + u_1 = 0, \\ & 1 + u_0 + u_1 + u_2 = 0,\\ \end{align} \tag{15} $$
求解这个问题可以得到：$u_0 = -\frac{4}{11}, u_1 = -\frac{3}{11}, u_3 = -\frac{4}{11}, u_4 = 0$。这个求解不太准，但控制的整体趋势没什么问题，为了能量最优，小球需要一直减速才能成功抵达目标的同时保证节约能量。采用 python 求解上述优化问题的结果如下：
from scipy.optimize import minimize
import numpy as np
e = 1e-10
fun = lambda x : 0.5 * x[0] ** 2 + x[1] ** 2 + x[2] ** 2 + 0.5 * x[3] ** 2
# 约束函数
cons = ({'type': 'eq', 'fun': lambda x: 1 + 2 * x[0] + x[1]},
{'type': 'eq', 'fun': lambda x: 1 + x[0] + x[1] + x[2]},
)
x0 = np.array((0.0, -1.0, 1.0, 0.0))
res = minimize(fun, x0, method='SLSQP', constraints=cons)
print('最小值：',res.fun)
print('最优解：',res.x)
print('迭代终止是否成功：', res.success)
print('迭代终止原因：', res.message)
# 输出结果
# 最小值： 0.2727277221226802
# 最优解： [-3.63922210e-01 -2.72155580e-01 -3.63922210e-01 -8.58968497e-09]
# 迭代终止是否成功： True
# 迭代终止原因： Optimization terminated successfully
使用 collocation 法求解
在 collocation 法中，我们不需要自行 rollout 得到 $u_i$ 之间的关系，可直接写出优化问题为：
$$ \begin{align} \min_{u_0, u_1, u_2, u_3, x_1, x_2, x_3, v_1, v_2, v_3} &\frac{1}{2}u^2_0 + u^2_1 + u^2_2 + \frac{1}{2} u^2_3 \\ \textrm{s.t.} \quad & x_{1} = x_0 + v_0 \\ & v_{1} = v_0 + u_0, \\ & x_{2} = x_1 + v_1, \\ & v_{2} = v_1 + u_1, \\ & x_{3} = x_2 + v_2, \\ & v_{3} = v_2 + u_2, \\ & x_3 = 0, \\ & v_3 = 0, \\ \end{align} \tag{16} $$
直接求解上述问题：
from scipy.optimize import minimize
import numpy as np
e = 1e-10
# u0, u1, u2, u3, x1, x2, x3, v_1, v_2, v_3
# 0 1 2 3 4 5 6 7 8 9
fun = lambda x : 0.5 * x[0] ** 2 + x[1] ** 2 + x[2] ** 2 + 0.5 * x[3] ** 2
# 约束函数
cons = ({'type': 'eq', 'fun': lambda x: x[4] + 2 - 1},
{'type': 'eq', 'fun': lambda x: x[5] - x[4] - x[7]},
{'type': 'eq', 'fun': lambda x: x[6] - x[5] - x[8]},
{'type': 'eq', 'fun': lambda x: x[6]},
{'type': 'eq', 'fun': lambda x: x[7] - 1 - x[0]},
{'type': 'eq', 'fun': lambda x: x[8] - x[7] - x[1]},
{'type': 'eq', 'fun': lambda x: x[9] - x[8] - x[2]},
{'type': 'eq', 'fun': lambda x: x[9]},
)
x0 = np.array((-0.3, -0.3, -0.3, 0.0, -2, -1, 0, 1, 0.5, 0)) # 设置初始值
res = minimize(fun, x0, method='SLSQP', constraints=cons)
print('最小值：',res.fun)
print('最优解：',res.x)
print('迭代终止是否成功：', res.success)
print('迭代终止原因：', res.message)
# 最小值： 0.2727272727272728
# 最优解： [-3.63636362e-01 -2.72727275e-01 -3.63636362e-01 -7.97038467e-09
# -1.00000000e+00 -3.63636362e-01 1.08420217e-19 6.36363638e-01
# 3.63636362e-01 -8.67361738e-19]
# 迭代终止是否成功： True
# 迭代终止原因： Optimization terminated successfully
得到的答案与在 shooting 法中的求解结果一样。我们现在将时间离散化为 100 个时刻求解，代码如下
from copy import deepcopy
from scipy.optimize import minimize
import numpy as np
e = 1e-10
N = 100 # 离散为 100 个间隔
H = 3 / N
def fun(x):
res = 0.5*(x[0]**2 * H + x[N]**2 * H)
for i in range(1, N):
res += x[i] ** 2 * H
return res
# 约束函数
cons = [{'type': 'eq', 'fun': lambda x: x[N+1] + 2 - 1 * H},
{'type': 'eq', 'fun': lambda x: x[N+N]},
{'type': 'eq', 'fun': lambda x: x[N+N+1] - 1 - x[0] * H},
{'type': 'eq', 'fun': lambda x: x[N+N+N]},
]
# 继续添加 x 的约束
def cons1(i):
return {'type': 'eq', 'fun': lambda x: x[i] - x[i-1] - x[i-1+N] * H}
cons += map(cons1, list(range(N+2, N+N+1)))
# 继续添加 v 的约束
def cons2(i): #列写第二种约束
return {'type': 'eq', 'fun': lambda x: x[i] - x[i-1] - x[i-N-N-1] * H}
cons += map(cons2, list(range(N+N+2, N+N+N+1)))
# 构造初始值
x0 = np.zeros((3*N +1,))
# 求解
res = minimize(fun, x0, method='SLSQP', constraints=cons)
print('最小值：',res.fun)
print('最优解：',res.x)
print('迭代终止是否成功：', res.success)
print('迭代终止原因：', res.message)
运行结果为：
最小值： 0.43788364734945745
最优解： [-2.42585830e-02 -2.05276815e-02 -2.65706264e-02 -3.26919795e-02
-3.88821857e-02 -4.51328843e-02 -5.14361228e-02 -5.77849002e-02
-6.41726353e-02 -7.05936098e-02 -7.70425318e-02 -8.35146694e-02
-9.00057432e-02 -9.65120771e-02 -1.03030215e-01 -1.09557169e-01
-1.16090359e-01 -1.22627476e-01 -1.29166510e-01 -1.35705868e-01
-1.42243965e-01 -1.48779710e-01 -1.55311827e-01 -1.61839736e-01
-1.68362698e-01 -1.74880181e-01 -1.81391747e-01 -1.87897304e-01
-1.94396663e-01 -2.00889715e-01 -2.07376486e-01 -2.13857215e-01
-2.20331991e-01 -2.26801168e-01 -2.33264840e-01 -2.39723557e-01
-2.46177518e-01 -2.52627133e-01 -2.59072744e-01 -2.65514932e-01
-2.71954045e-01 -2.78390474e-01 -2.84824660e-01 -2.91257071e-01
-2.97688161e-01 -3.04118254e-01 -3.10547764e-01 -3.16977228e-01
-3.23406884e-01 -3.29837270e-01 -3.36268523e-01 -3.42701211e-01
-3.49135429e-01 -3.55571614e-01 -3.62010062e-01 -3.68450955e-01
-3.74894597e-01 -3.81341282e-01 -3.87791005e-01 -3.94244121e-01
-4.00700762e-01 -4.07161054e-01 -4.13625050e-01 -4.20092960e-01
-4.26564875e-01 -4.33040723e-01 -4.39520606e-01 -4.46004653e-01
-4.52492619e-01 -4.58984646e-01 -4.65480657e-01 -4.71980495e-01
-4.78484120e-01 -4.84991316e-01 -4.91501985e-01 -4.98015842e-01
-5.04532818e-01 -5.11052533e-01 -5.17574735e-01 -5.24098966e-01
-5.30625014e-01 -5.37152345e-01 -5.43680493e-01 -5.50208894e-01
-5.56737087e-01 -5.63264185e-01 -5.69789737e-01 -5.76312832e-01
-5.82832568e-01 -5.89348088e-01 -5.95858342e-01 -6.02362183e-01
-6.08858360e-01 -6.15345603e-01 -6.21822330e-01 -6.28287014e-01
-6.34737984e-01 -6.41173371e-01 -6.47591023e-01 -6.53988900e-01
-5.57497781e-09, # 前面是101个控制量
-1.97000000e+00, ..., 0.00000000e+00, # 位置
9.99272243e-01, ..., 0.00000000e+00] # 速度
迭代终止是否成功： True
迭代终止原因： Optimization terminated successfully
这个结果已经很接近最优解了（上式前 101 个数值对应 $u_0,…,u_{100}$），可自行通过 Pontryagin 极小值原理求解析解进行验证 (最优控制率是 $-\frac{2}{9} t$，最优值是 $0.444444$)，因此最优控制的力的大小与小球运动方向相反，力的大小从 $0$ 逐步增加到 $-0.6667$。
总之，对于最优控制的数值求解，如何对微分方程离散化是重点，最简单的离散化方法是一阶欧拉法，进一步的有龙格库塔法。更加普适的离散化方法可以理解为先对状态进行插值，后配点保证状态方程的约束得到满足。另外，非线性求解也相当重要，不过现有求解方法都大同小异，商用版本和开源版本的区别在于商用版对初值可能猜的更准一点，导致商用求解器求得更准。
利用开源最优控制库 ACADO 求解上述问题
首先，你需要根据官方文档下载和编译 ACADO。下面是上述例子的求解代码：
#include "utils/acado_types.hpp"
#include "variables_grid/variables_grid.hpp"
#include <acado_optimal_control.hpp>
int main() {
USING_NAMESPACE_ACADO
DifferentialState x, v; // 状态变量 x, v
Control u; // 控制量 u
DifferentialEquation f(0.0, 3.0); // 微分方程 0 - 3 秒
//-------------------------------------
OCP ocp(0.0, 3.0);
ocp.minimizeLagrangeTerm(u * u); // 优化目标为能量最优
f << dot(x) == v; // 状态方程
f << dot(v) == u; // 状态方程
ocp.subjectTo(f); // ocp 要满足状态方程约束
ocp.subjectTo(AT_START, x == -2.0); // 初值约束
ocp.subjectTo(AT_START, v == 1.0);
ocp.subjectTo(AT_END, x == 0.0); // 终点约束
ocp.subjectTo(AT_END, v == 0.0);
//-------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm.set(INTEGRATOR_TYPE, INT_RK78); // 龙格库塔
algorithm.set(INTEGRATOR_TOLERANCE, 1e-8);
algorithm.set(DISCRETIZATION_TYPE, COLLOCATION); // 采用配点法
algorithm.set(KKT_TOLERANCE, 1e-5);
algorithm.solve(); // and solve the problem.
// 求解结果写入文件
algorithm.getDifferentialStates("../results/states.txt");
algorithm.getControls("../results/controls.txt");
std::cout << algorithm.getObjectiveValue() << std::endl;
return 0;
}
得到的结果如下：
-
最优值为：
4.447228e-01，与我们通过解析算出来的非常接近。 -
0-3s的控制量为：
时间 控制量
0.0000000000000000e+00 -1.5871620685966214e-02
1.5000000000000002e-01 -4.9288144456670770e-02
3.0000000000000004e-01 -8.2693689973987142e-02
4.4999999999999996e-01 -1.1611097339982829e-01
6.0000000000000009e-01 -1.4954221409022475e-01
7.5000000000000000e-01 -1.8296142557594697e-01
8.9999999999999991e-01 -2.1639900419137911e-01
1.0499999999999998e+00 -2.4979498753721793e-01
1.2000000000000002e+00 -2.8320332095006667e-01
1.3500000000000001e+00 -3.1663701648297415e-01
1.5000000000000000e+00 -3.5005302091599860e-01
1.6500000000000001e+00 -3.8345424490992930e-01
1.7999999999999998e+00 -4.1685299967605571e-01
1.9500000000000002e+00 -4.5028378057125112e-01
2.0999999999999996e+00 -4.8370225579944504e-01
2.2500000000000000e+00 -5.1714261705316311e-01
2.4000000000000004e+00 -5.5056992795918158e-01
2.5499999999999998e+00 -5.8397620092103697e-01
2.7000000000000002e+00 -6.1733988265827267e-01
2.8499999999999996e+00 -6.5078933885806411e-01
3.0000000000000000e+00 -6.5078933885806411e-01
- 控制中的状态量为：
时间 位置 速度
0.0000000000000000e+00 -2.0000000000000000e+00 1.0000000000000000e+00
1.5000000000000002e-01 -1.8501785557327175e+00 9.9761925689710484e-01
3.0000000000000004e-01 -1.7010901588232894e+00 9.9022603522860442e-01
4.4999999999999996e-01 -1.5534865575512053e+00 9.7782198173250623e-01
6.0000000000000009e-01 -1.4081195087420766e+00 9.6040533572253217e-01
7.5000000000000000e-01 -1.2657410582922122e+00 9.3797400360899885e-01
8.9999999999999991e-01 -1.1271032737885922e+00 9.1052978977260679e-01
1.0499999999999998e+00 -9.9295829411985381e-01 8.7806993914390008e-01
1.2000000000000002e+00 -8.6405799685806184e-01 8.4060069101331714e-01
1.3500000000000001e+00 -7.4115393056675227e-01 7.9812019287080704e-01
1.5000000000000000e+00 -6.2499806807156477e-01 7.5062464039836108e-01
1.6500000000000001e+00 -5.1634246849711585e-01 6.9811668726096099e-01
1.7999999999999998e+00 -4.1593882566320856e-01 6.4059855052447179e-01
1.9500000000000002e+00 -3.2453863933089305e-01 5.7807060057306325e-01
2.0999999999999996e+00 -2.4289374177636014e-01 5.1052803348737552e-01
2.2500000000000000e+00 -1.7175618713099730e-01 4.3797269511745857e-01
2.4000000000000004e+00 -1.1187813730522635e-01 3.6040130255948377e-01
2.5499999999999998e+00 -6.4011853610844688e-02 2.7781581336560668e-01
2.7000000000000002e+00 -2.8909213866365257e-02 1.9021938322745069e-01
2.8499999999999996e+00 -7.3213800621532779e-03 9.7618400828709997e-02
3.0000000000000000e+00 2.2002975643862965e-20 6.2740633170670801e-21