离散系统的最优控制

1. 调节系统

对下面的离散系统
$$\vec{x}(k+1)=F\vec{x}(k)+G\vec{u}(k)$$

1.1 推导

给定二次型性能指标函数
$$J={\vec{x}}^{\text{T}}(N)Q_0\vec{x}(N)+\sum _{k=0}^{N-1}[{\vec{x}}^{\text{T}}(k)Q_1\vec{x}(k)+{\vec{u}}^{\text{T}}(k)Q_2\vec{u}(k)]$$
使用动态规划法，定义 $J_i$ 并找到 $J_i$ 和 $J_{i+1}$ 之间的递推关系
$$\begin{array}{rl}{J}_i=& {\vec{x}}^{\text{T}}(N)Q_0\vec{x}(N)+\sum _{k=i}^{N-1}[{\vec{x}}^{\text{T}}(k)Q_1\vec{x}(k)+{\vec{u}}^{\text{T}}(k)Q_2\vec{u}(k)]\\ =& {\vec{x}}^{\text{T}}(N)Q_0\vec{x}(N)+\sum _{k=i+1}^{N-1}[{\vec{x}}^{\text{T}}(k)Q_1\vec{x}(k)+{\vec{u}}^{\text{T}}(k)Q_2\vec{u}(k)]\\ & +{\vec{x}}^{\text{T}}(i)Q_1\vec{x}(i)+{\vec{u}}^{\text{T}}(i)Q_2\vec{u}(i)\\ =& J_{i+1}+{\vec{x}}^{\text{T}}(i)Q_1\vec{x}(i)+{\vec{u}}^{\text{T}}(i)Q_2\vec{u}(i)\end{array}$$
然后从后往前开始求最优解
$$\begin{array}{rl}{J}_N=& {\vec{x}}^{\text{T}}(N)Q_0\vec{x}(N)\\ J_{N-1}=& J_N+{\vec{x}}^{\text{T}}(N-1)Q_1\vec{x}(N-1)+{\vec{u}}^{\text{T}}(N-1)Q_2\vec{u}(N-1)\end{array}$$
此时求解 $\vec{u}(N-1)$ 使 $J_{N-1}$ 最小，把 $J_{N-1}$ 对 $\vec{u}(N-1)$ 求导
$$\begin{array}{rl}\frac{\text{d}{J}_N}{\text{d}\vec{u}(N-1)}=& \frac{\text{d}\vec{x}(N{)}^{\text{T}}}{\text{d}\vec{u}(N-1)}\cdot \frac{\text{d}{J}_N}{\text{d}\vec{x}(N)}=G^{\text{T}}\cdot (Q_0+Q_0^{\text{T}})\vec{x}(N)\\ \frac{\text{d}{J}_{N-1}}{\text{d}\vec{u}(N-1)}=& \frac{\text{d}{J}_N}{\text{d}\vec{u}(N-1)}+\frac{\text{d}}{\text{d}\vec{u}(N-1)}[{\vec{x}}^{\text{T}}(N-1)Q_1\vec{x}(N-1)+{\vec{u}}^{\text{T}}(N-1)Q_2\vec{u}(N-1)]\\ =& G^{\text{T}}(Q_0+Q_0^{\text{T}})\vec{x}(N)+(Q_2+Q_2^{\text{T}})\vec{u}(N-1)\\ =& 2G^{\text{T}}{Q}_0\vec{x}(N)+2Q_2\vec{u}(N-1)\end{array}$$
令导数为0得到
$$\begin{array}{rl}& G^{\text{T}}{Q}_0\vec{x}(N)+Q_2\vec{u}(N-1)=0\\ & G^{\text{T}}{Q}_0[F\vec{x}(N-1)+G\vec{u}(N-1)]+Q_2\vec{u}(N-1)=0\\ & G^{\text{T}}{Q}_{0}F\vec{x}(N-1)+[G^{\text{T}}{Q}_{0}G+Q_2]\vec{u}(N-1)=0\\ & \vec{u}(N-1)=-[G^{\text{T}}{Q}_{0}G+Q_2{]}^{-1}{G}^{\text{T}}{Q}_{0}F\vec{x}(N-1)\end{array}$$
令
$$\begin{array}{rl}S(N)=& Q_0\\ L(N-1)=& [G^{\text{T}}{Q}_{0}G+Q_2{]}^{-1}{G}^{\text{T}}{Q}_{0}F\\ =& [G^{\text{T}}S(N)G+Q_2{]}^{-1}{G}^{\text{T}}S(N)F\end{array}$$
则
$$\begin{array}{rl}\vec{u}(N-1)=& -L(N-1)\vec{x}(N-1)\\ {\vec{x}}^{\text{T}}(N)=& [F\vec{x}(N-1)+G\vec{u}(N-1){]}^{\text{T}}\\ =& {\vec{x}}^{\text{T}}(N-1)[F-GL{]}^{\text{T}}\end{array}$$
带入 $J(N-1)$ 得到
$$\begin{array}{rl}J(N-1)=& {\vec{x}}^{\text{T}}(N)Q_0\vec{x}(N)+[{\vec{x}}^{\text{T}}(N-1)Q_1\vec{x}(N-1)+{\vec{u}}^{\text{T}}(N-1)Q_2\vec{u}(N-1)]\\ =& {\vec{x}}^{\text{T}}(N-1)(F-GL(N-1){)}^{\text{T}}{Q}_0(F-GL(N-1))\vec{x}(N-1)\\ & +{\vec{x}}^{\text{T}}(N-1)Q_1\vec{x}(N-1)+{\vec{x}}^{\text{T}}(N-1)L(N-1{)}^{\text{T}}{Q}_{2}L(N-1)\vec{x}(N-1)\\ =& {\vec{x}}^{\text{T}}(N-1)S(N-1)\vec{x}(N-1)\end{array}$$
因为括号里都是 $(N-1)$，所以下面用省略括号的简洁版
$$\begin{array}{rl}J(N-1)=& {\vec{x}}^{\text{T}}(F-GL{)}^{\text{T}}{Q}_0(F-GL)\vec{x}+{\vec{x}}^{\text{T}}{Q}_{1}x+{\vec{x}}^{\text{T}}{L}^{\text{T}}{Q}_{2}Lx={\vec{x}}^{\text{T}}S\vec{x}\end{array}$$
其中
$$S(N-1)=(F-GL{)}^{\text{T}}{Q}_0(F-GL)+Q_1+L^{\text{T}}{Q}_{2}L$$
继续算 $N-2$
$$\begin{array}{rl}{J}_{N-2}=& J_{N-1}+{\vec{x}}^{\text{T}}(N-2)Q_1\vec{x}(N-2)+{\vec{u}}^{\text{T}}(N-2)Q_2\vec{u}(N-2)\\ \frac{\text{d}{J}_{N-2}}{\text{d}\vec{u}(N-2)}=& \frac{\text{d}{J}_{N-1}}{\text{d}\vec{u}(N-2)}+\frac{\text{d}}{\text{d}\vec{u}(N-2)}[{\vec{u}}^{\text{T}}(N-2)Q_2\vec{u}(N-2)]\\ =& \frac{\text{d}}{\text{d}\vec{u}(N-2)}\left[{\vec{x}}^{\text{T}}S\vec{x}\right]+2Q_2\vec{u}(N-2)\\ =& \frac{\text{d}{\vec{x}}^{\text{T}}(N-1)}{\text{d}\vec{u}(N-2)}[2S(N-1)\vec{x}(N-1)]+2Q_2\vec{u}(N-2)\\ \frac{1}{2}\frac{\text{d}{J}_{N-2}}{\text{d}\vec{u}(N-2)}=& \frac{\text{d}[G\vec{u}(N-2){]}^{\text{T}}}{\text{d}\vec{u}(N-2)}[S(N-1)\vec{x}(N-1)]+Q_2\vec{u}(N-2)\\ =& G^{\text{T}}S(N-1)[F\vec{x}(N-2)+G\vec{u}(N-2)]+Q_2\vec{u}(N-2)\end{array}$$
令导数为0得到
$$\begin{array}{rl}& [G^{\text{T}}S(N-1)G+Q_2]\vec{u}(N-2)+G^{\text{T}}S(N-1)F\vec{x}(N-2)=0\\ & L(N-2)=[G^{\text{T}}S(N-1)G+Q_2{]}^{-1}{G}^{\text{T}}S(N-1)F\\ & \vec{u}(N-2)=-L(N-2)\vec{x}(N-2)\end{array}$$
依次类推得到每个时刻的 $\vec{u}$（这我不会先空着）
$$\begin{array}{rl}\frac{\text{d}{J}_i}{\text{d}{\vec{u}}_i}=& \frac{\text{d}{J}_{i+1}}{\text{d}{\vec{u}}_i}+\frac{\text{d}}{\text{d}{\vec{u}}_i}[{\vec{x}}^{\text{T}}(i)Q_1\vec{x}(i)+{\vec{u}}^{\text{T}}(i)Q_2\vec{u}(i)]\\ =& \end{array}$$
递推关系
$$\begin{array}{rl}L[k]=& (G^{\text{T}}S[k+1]G+Q_2{)}^{-1}{G}^{\text{T}}S[k+1]F\\ S[k]=& (F-GL[k]{)}^{\text{T}}{Q}_0(F-GL[k])+L^{\text{T}}[k]Q_{2}L[k]+Q_1\end{array}$$

1.2 仿真

# 离散最优控制
import numpy as np
import matplotlib.pyplot as plt
N = 128
STEP = 0.1
F = np.matrix([[1, STEP], [0, 1]])
G = np.matrix([[0.5*STEP**2], [STEP]])
Q_0 = np.matrix([[1, 0], [0, 1]])
Q_1 = 0.01 * np.matrix([[1, 0], [0, 1]])
Q_2 = 0.01
vecx = np.matrix([[10], [5.5]])
S = Q_0
listL = []
for n1 in range(N):
    coeff = 1 / (G.T @ S @ G + Q_2)[0,0]
    L = coeff * G.T @ S @ F
    S = (F - G @ L).T @ Q_0 @ (F - G @ L) + Q_1 + Q_2 * L.T @ L
    listL = [*listL, L]
plotdata = []
for n1 in range(N):
    u = -(listL[n1] @ vecx)[0,0]
    vecx = F @ vecx + G * u
    plotdata.append([n1, vecx[0,0], vecx[1,0]])
plotdata = np.array(plotdata)
plt.plot(plotdata[:, 0], plotdata[:, 1])
plt.plot(plotdata[:, 0], plotdata[:, 2])
plt.show()

2 跟踪系统

（待补充。。。）

向量求导公式

$$\begin{array}{rl}& \frac{\text{d}}{\text{d}\vec{x}}({\vec{x}}^{\text{T}}A\vec{x})=(A+A^{\text{T}})x\\ & \frac{\text{d}{\vec{x}}^{\text{T}}}{\text{d}\vec{x}}=I\\ & \frac{\text{d}\vec{x}}{\text{d}\vec{x}}=\vec{I}={\left[\begin{array}{ccccccccc}1& 0& 0,& 0& 1& 0,& 0& 0& 1\end{array}\right]}^{\text{T}}\\ & \frac{\text{d}(A\vec{x}{)}^{\text{T}}}{\text{d}\vec{x}}=A^{\text{T}}\\ & \frac{\text{d}A\vec{x}}{\text{d}\vec{x}}=\vec{A}={\left[\begin{array}{ccc}{\vec{a}}_1^{\text{T}}& {\vec{a}}_2^{\text{T}}& {\vec{a}}_3^{\text{T}}\end{array}\right]}^{\text{T}}\\ & \frac{\text{d}f}{\text{d}\vec{x}}=\frac{\text{d}{\vec{y}}^{\text{T}}}{\text{d}\vec{x}}\cdot \frac{\text{d}f}{\text{d}\vec{y}}\end{array}$$

参考

最优控制理论 九、Bellman动态规划法用于最优控制 -优快云博客