Python 还原控制SCI论文算法系列1：基于策略迭代的自适应最优控制器设计

文章目录

Python 还原控制SCI论文算法系列1：基于策略迭代的自适应最优控制器设计
- 0.前言
- 1.研究问题的描述
- - 1.1 经典线性系统最优控制器设计问题
  - 1.2 策略迭代问题的提出
- 2. 论文所提的策略迭代控制算法详解
- - 2.1 idea1: 将部分价值函数等效成Lyapunov函数增量
  - 2.2 idea2:最小二乘法估算P
  - 2.3 A-C 结构的控制器在线更新策略
- 3. 论文仿真案例的python编程还原
- - 3.1 系统模型建模
  - 3.2 Python程序
  - - 3.2.1 功能包
    - 3.2.2 构建环境函数
    - 3.2.3 Critic-Actor 策略更新函数
    - 3.2.4 主程序
    - 3.2.5 轨迹图
- 4. 发现的问题
- 5 结语

0.前言

经过前两个文章的基本铺垫，

Python解决微分方程-状态方程求解作图问题

Python解决线性连续系统最优控制状态反馈问题

从本系列开始，我将把写作重心倾向于复现目前所研究的SCI科研论文提出的控制算法。本文将关注强化学习与最优控制领域的经典控制文章：

D.Vrabie, O.Pastravanu, M. Abu-Khalaf, F.L. Lewis. Adaptive optimal control for continuous-time linear systems based on policy iteration, Automatica, 2009(45): 477-484

文章将描述Lewis团队所提出的策略迭代算法相比于传统最优控制算法的创新点，并通过python编程将策略迭代控制算法程序化，通过文章中的例子实现算法复现，最后根据复现过程中发现的数值精度问题，提出本文中算法执行时还存在什么样的问题。

1.研究问题的描述

1.1 经典线性系统最优控制器设计问题

对于线性系统:
x˙=Ax+Bu（1）\dot{x} = A x + B u （1） x˙=Ax+Bu（1）
问题的核心是求一个最优控制器u∗=−K∗xu^{*} = -K^{*} xu∗=−K∗x,使得：

（i）系统的状态能够在反馈控制作用下收敛到平衡点；

（ii）系统的价值函数：
J=∫t0∞xT(τ)Qx(τ)+uT(τ)Ru(τ)dτ（2）J = \int^{\infin}_{t_0} x^T(\tau)Qx(\tau) + u^T(\tau)Ru(\tau)d\tau （2） J=∫t0∞xT(τ)Qx(τ)+uT(τ)Ru(τ)dτ（2）
在控制器作用下值取到最小。

传统的求解方法即只需解出经典的代数黎卡提方程的解P即可：
ATP+PA−PBR−1BTP+Q=0（3）A^TP+PA-PBR^{-1}B^{T}P+Q = 0 （3） ATP+PA−PBR−1BTP+Q=0（3）
对应的最优控制器u∗u^{*}u∗可以表示为：
u∗=−K∗x=−R−1BTPx（4）u^{*} = - K^* x = -R^{-1}B^TPx （4） u∗=−K∗x=−R−1BTPx（4）

1.2 策略迭代问题的提出

从式（3）可以很明显的看出，如果要求出代数黎卡提方程的解P，则必须知道系统矩阵A，控制矩阵B和价值函数中的Q与R的精确值，从而进一步由（4）推导得到最优控制器。如果系统矩阵A未知时，那么很明显，直接使用代数黎卡提方程（3）来求解P将变的无法进行。因此，必须要寻找到一种新的算法，能够解决系统矩阵A不精确已知时最优控制器的求法。这就是上述论文需要核心解决的问题。

2. 论文所提的策略迭代控制算法详解

2.1 idea1: 将部分价值函数等效成Lyapunov函数增量

目前我们已知了最优控制器的结构为（4）式，唯一需要解决的是P∗P^{*}P∗的值。因此，可以考虑用P的迭代值来代替精确值，保证迭代足够多次后，P能够收敛到精确值P∗P^{*}P∗。

为此，假设存在一个K能够保证A -BK为Hurwitz矩阵，即特征值实部为负，且满足
V(t)=xT(t)Px(t)=∫t∞xT(τ)Qx(τ)+xT(τ)KTRKx(τ)dτ（5）V(t) = x^T(t)Px(t) = \int^{\infin}_{t}x^T(\tau)Qx(\tau)+x^T(\tau)K^TRKx(\tau)d\tau （5） V(t)=xT(t)Px(t)=∫t∞xT(τ)Qx(τ)+xT(τ)KTRKx(τ)dτ（5）
P为Lyapunov方程的解，即
(A−BK)TP+P(A−BK)=Q+KTRK（6）(A-BK)^TP+P(A-BK) = Q + K^TRK （6） (A−BK)TP+P(A−BK)=Q+KTRK（6）

那么，可以将价值函数进行如下等效变换：
V(t)=∫tt+TxT(τ)(Q+KTRK)x(τ)dτ+V(t+T)（7）V(t) =\int^{t+T}_{t} x^T(\tau)(Q + K^TRK) x(\tau)d\tau + V(t + T) （7） V(t)=∫tt+TxT(τ)(Q+KTRK)x(τ)dτ+V(t+T)（7）
结合（5）式，可得
xT(t)Px(t)−xT(t+T)Px(t+T)=∫tt+TxT(τ)(Q+KTRK)x(τ)dτ（6）x^T(t)Px(t) - x^T(t+T)Px(t+T) = \int^{t+T}_{t}x^T(\tau)(Q+K^TRK)x(\tau)d\tau （6） xT(t)Px(t)−xT(t+T)Px(t+T)=∫tt+TxT(τ)(Q+KTRK)x(τ)dτ（6）
定义

xˉ=(x12,x1x2,...,x1xn,x22,x2x3,...,x2xn,...,xn2)T∈Rn(1+n)2\bar{x} = (x_1^2, x_1x_2, ... , x_1x_n, x_2^2, x_2x_3,...,x_2x_n, ..., x_n^2)^T \in R^{\frac{n(1+n)}{2}}xˉ=(x12,x1x2,...,x1xn,x22,x2x3,...,x2xn,...,xn2)T∈R2n(1+n), xˉΔ=x(t)−x(t+T)\bar{x}_{\Delta} = x(t) - x(t+T)xˉΔ=x(t)−x(t+T),

Pˉ=(P11,2P12,...,2P1n,P22,2P23,...,2P2n,P33,...,Pnn)T∈Rn(1+n)2\bar{P} = (P_{11}, 2P_{12}, ..., 2P_{1n}, P_{22}, 2P_{23},...,2P_{2n}, P_{33},...,P_{nn})^T\in R^{\frac{n(1+n)}{2}}Pˉ=(P11,2P12,...,2P1n,P22,2P23,...,2P2n,P33,...,Pnn)T∈R2n(1+n),

那么（8）式可以化解为：
PˉTxˉΔ=∫tt+TxT(τ)(Q+KTRK)x(τ)dτ（9）\bar{P}^T\bar{x}_{\Delta} = \int^{t+T}_{t}x^T(\tau)(Q+K^TRK)x(\tau)d\tau （9） PˉTxˉΔ=∫tt+TxT(τ)(Q+KTRK)x(τ)dτ（9）
在（9）式基础上，可以考虑采用P的迭代策略伪代码：

（i）初始化 P0=0P_0 = 0P0=0, $K_1 $满足 $A - BK_1 $是Hurwitz的；

（II）Loop 从 i = 1， … , ∞\infin∞开始：

通过 PˉiTxˉΔ=∫tt+TxT(τ)(Q+KiTRKii)x(τ)dτ\bar{P}_i^T\bar{x}_{\Delta} = \int^{t+T}_{t}x^T(\tau)(Q+K^T_iRKi_i)x(\tau)d\tauPˉiTxˉΔ=∫tt+TxT(τ)(Q+KiTRKii)x(τ)dτ更新PiP_iPi，采样点个数N要大于 n(n+1)2\frac{n(n+1)}{2}2n(n+1)；

如果∣∣Pi−Pi−1∣∣<ϵ||P_{i }- P_{i-1}|| < \epsilon∣∣Pi−Pi−1∣∣<ϵ:

结束迭代，跳出循环；
否则通过Ki+1=R−1BTPiK_{i+1} = R^{-1}B^TP_iKi+1=R−1BTPi实现控制器增益的更新；
endLoop

策略迭代算法的核心是在一定时间内每隔时间间隔T取N个系统状态点xˉΔ1,...,xˉΔN\bar{x}_{\Delta}^{1}, ..., \bar{x}_{\Delta}^{N}xˉΔ1,...,xˉΔN，从而在当前控制策略KiK_iKi下可以评估出当前策略下的PiP_iPi是否取的合适。到此为止，算法实现的最大问题就落在如何解决评估P时的积分问题。

2.2 idea2:最小二乘法估算P

要实现（9），还存在一个难点就是估算（9）等号右边的积分价值函数。解决的思路是把积分当作微分方程来处理，即令

存在一个关于时间t连续的函数v(t)v(t)v(t), 满足：
v˙(t)=xT(τ)(Q+KTRK)x(τ)（10）\dot v(t) = x^T(\tau)(Q + K^TRK)x(\tau) （10） v˙(t)=xT(τ)(Q+KTRK)x(τ)（10）
这样（9）式就可以等价为
PˉTxˉΔ=∫tt+TxT(τ)(Q+KTRK)x(τ)dτ=v(t+T)−v(t)=d(xˉΔ,k)（11）\bar{P}^T\bar{x}_{\Delta} = \int^{t+T}_{t}x^T(\tau)(Q+K^TRK)x(\tau)d\tau=v(t+T) -v(t)=d(\bar{x}_{\Delta}, k) （11） PˉTxˉΔ=∫tt+TxT(τ)(Q+KTRK)x(τ)dτ=v(t+T)−v(t)=d(xˉΔ,k)（11）
(11)式是可以通过求（10）式的微分方程得到采样时刻端点处的数值近似解的，因此在编程时只要把（10）式作为（1）式的附加系统一起进行微分方程数值计算处理即可。考虑到在每一次策略评估时，需要对系统状态（包含（10）中的v(t)）采样N个点，因此令

X=(xˉΔ1,...,xˉΔN)TX = (\bar{x}_{\Delta}^1, ..., \bar{x}_{\Delta}^N)^TX=(xˉΔ1,...,xˉΔN)T, Y=(d(xˉΔ1,ki),...,d(xˉΔN,ki))TY = (d(\bar{x}_{\Delta}^{1}, k_i), ...,d(\bar{x}_{\Delta}^{N}, k_i))^TY=(d(xˉΔ1,ki),...,d(xˉΔN,ki))T，

(11)式就变为：
XTPˉi=Y（12）X^T\bar{P}_i = Y （12） XTPˉi=Y（12）
于是可以使用最小二乘法，来求得近似数值解
Piˉ=(XXT)−1XY（13）\bar{P_i} = (XX^T)^{-1}XY （13） Piˉ=(XXT)−1XY（13）

2.3 A-C 结构的控制器在线更新策略

有了2.1和2.2的分析基础，现在我们可以得到基于Actor-Critic结构的强化学习控制器在线更新策略：

Environment(环境)：被控系统（1）+ 附加系统（10），接收Actor产生的新策略，反馈给Critic N个状态采样点的信息；

Critic(评估者)：负责评估当前KiK_iKi控制策略下的PiP_iPi值，评估方法为：从t时刻开始，每隔T对系统状态和附加状态采样，当收集到N个采样点后，根据（12）式利用最小二乘法评估出近似的解PiP_iPi；

Actor(执行者): 负责根据评估的结果，产生新的控制策略Ki+1=R−1BTPiK_{i+1} = R^{-1}B^TP_{i}Ki+1=R−1BTPi, 作用系统中产生的新的数据点，控评估者进行评估。

3. 论文仿真案例的python编程还原

3.1 系统模型建模

论文的环境模型采用的是电力系统中在可操作点处的近似线性模型。模型的标准系统矩阵AnA_nAn为
An=[−0.06658000−3.6633.6630−6.860−13.736−13.7360.6000]B=[0013.7360]TA_n = \begin{bmatrix} -0.0665 & 8 & 0 & 0\\ 0 & -3.663 & 3.663 & 0\\ -6.86 & 0 & -13.736 & -13.736\\ 0.6 & 0 & 0 & 0 \end{bmatrix}\\ B = \begin{bmatrix} 0 & 0 & 13.736 & 0 \end{bmatrix}^T An=⎣⎢⎢⎡−0.06650−6.860.68−3.6630003.663−13.736000−13.7360⎦⎥⎥⎤B=[0013.7360]T
在该模型下，可以通过解（3）式的代数黎卡提方程求得最优控制增益

K=[0.826689361.700305270.70494750.41421356]K=\begin{bmatrix}0.82668936 \quad 1.70030527 \quad 0.7049475 \quad 0.41421356\end{bmatrix}K=[0.826689361.700305270.70494750.41421356],

把它作为控制策略迭代的初始增益K1K_1K1。考虑此时电力系统的操作点发生了移动，则对应的线性化方程也会发生改变，系统矩阵不再是 AnA_nAn，而是变成了A：
A=[−0.066511.5000−2.52.50−9.50−13.736−13.7360.6000]A = \begin{bmatrix} -0.0665 & 11.5 & 0 & 0\\ 0 & -2.5 & 2.5 & 0\\ -9.5 & 0 & -13.736 & -13.736\\ 0.6 & 0 & 0 & 0 \end{bmatrix} A=⎣⎢⎢⎡−0.06650−9.50.611.5−2.50002.5−13.736000−13.7360⎦⎥⎥⎤
考虑此时如何运用策略迭代控制算法实现最优控制策略的生成同时保证系统仍然镇定。

3.2 Python程序

3.2.1 功能包

import numpy as np
import control as ct  #控制包
import matplotlib.pyplot as plt
from scipy import linalg, integrate #求微分方程
np.set_printoptions(suppress=True)  #小数不用科学计数法

3.2.2 构建环境函数

def env_lin_sys(t, state, A, B, K, Q, R):x, V = state[:4], state[4]dx1, dx2, dx3, dx4 = A @ x  - B @ K @ x # 系统模型dV = x.T @ (Q + K.T @ R @ K) @ x        # 附加模型（价值函数积分等效的微分方程）return [dx1, dx2, dx3, dx4, dV]         #返回环境中的系统状态

3.2.3 Critic-Actor 策略更新函数

# 该函数将从环境中获取的状态采样点求差，用于后续最小二乘法估算P_i
def calculate_delta_bx_V(state):x1, x2, x3, x4, V = stateN = len(x1)        bx = np.zeros((N, 10))delta_bx = np.zeros((N-1, 10))delta_V = np.zeros((N-1, ))for i in range(N):bx[i] = [x1[i]**2, x1[i]*x2[i], x1[i]*x3[i], x1[i]*x4[i], x2[i]**2, x2[i]*x3[i], x2[i]*x4[i], x3[i]**2, x3[i]*x4[i], x4[i]**2]   #计算每个采样点处的\bar{x}_xif i != 0:delta_bx[i - 1] = bx[i-1] - bx[i]   #计算出\bar{x}_{\Delta}delta_V[i - 1]  = V[i] - V[i - 1]   #计算出d(\bar{x}, k) return delta_bx, delta_V
# Critic 评估现有控制策略k_i下的P
def critic_lstsq(delta_bx, delta_V):bp, res, rank, s = linalg.lstsq(delta_bx, delta_V) #直接调用sci.linalg.lstsq的现成最小二乘法函数，比通过解析解算的结果精度更高。p = np.array([[bp[0], bp[1]/2, bp[2]/2, bp[3]/2],[bp[1]/2, bp[4], bp[5]/2, bp[6]/2],[bp[2]/2, bp[5]/2, bp[7], bp[8]/2],[bp[3]/2, bp[6]/2, bp[8]/2, bp[9]]    ])return bp, p
#策略迭代
def policy_iteration(state, bp_current, p_current, R, B, eps):deltabx, deltaV = calculate_delta_bx_V(state)bp_new, p_new = critic_lstsq(deltabx, deltaV)policy_terminal = False# 评估相邻两次的P是否接近print(linalg.norm(bp_new - bp_current))if linalg.norm(bp_new - bp_current) < eps:p_new = p_currentbp_new = bp_currentpolicy_terminal = TrueK_new = linalg.inv(R) @ B.T @ p_new  #Actor 更新控制策略return K_new, p_new, bp_new, policy_terminal
#选择初始策略K
def init_k(A, B, Q, R, init_ctrl=False):if init_ctrl:P, L, K = ct.care(A, B, Q, R)return Kelse:K = np.zeros((1,4))return K

3.2.4 主程序

# 主程序
# 设置环境参数（系统）
if __name__ == '__main__':A = np.array([[-0.0665, 11.5, 0, 0],[0, -2.5, 2.5, 0],[-9.5, 0, -13.736, -13.736],[0.6, 0, 0, 0]])An = np.array([[-0.0665, 8, 0, 0],[0, -3.663, 3.663, 0],[-6.86, 0, -13.736, -13.736],[0.6, 0, 0, 0]])B = np.array([[0], [0], [13.736], [0]])Q = np.diag([1.,1.,1,1.])R = np.diag([1.0])# 初始化P0 = np.zeros((4, 4))K1 = init_k(An, B, Q, R, init_ctrl=True) #K为原始系统的最优控制增益bP_current = np.zeros((10, ))traj_x = np.array([None])traj_t = np.array([None])init_x =  np.array([0.,0.1, 0, 0.]) #初始状态init_V = 100 #这个值可以随便设，对结果影响不大init_state = np.append(init_x, init_V)t = np.linspace(0, 1, 21)     # 每隔0.05秒从环境中采样一个点，1秒采21个点，包括初始点policy_terminal = FalseK_current = K1 P_current = P0#主循环，考虑仿真时间最多为8秒for i in range(8):#采用solve_ivp求解ode方程，每次计算时间为1s，每隔0.05秒采样一个点，固定步长0.001s，方法为龙格-库塔45traj_sol = integrate.solve_ivp(env_lin_sys, [i, i+1], init_state, args=(A, B,                           K_current, Q, R), t_eval=t, dense_output=True,method='RK45', first_step=0.001, max_step=0.001)#存储[0-1]秒的采样到的21个点对应的时间和数值，方便后续调用画图if i == 0:traj_t = traj_sol.ttraj_x = traj_sol.y[:4]else:traj_t = np.hstack([traj_t, np.delete(traj_sol.t, 0)])aug_traj = np.delete(traj_sol.y[:4], 0, axis=1) #每次初始点都会重合，拼接时要消去traj_x = np.concatenate((traj_x, aug_traj), axis=1)# 评估结果显示需继续策略更新if policy_terminal == False:K_new, p_new, dp_new, policy_terminal = policy_iteration(traj_sol.y, bP_current, P_current, R, B, eps=1e-2) #误差不能设置的过大，会影响收敛结果#策略更新P_current = p_newbP_current = dp_newK_current = K_new#初值更新，仿真时间区间更新init_state = traj_sol.y[ :, -1]t = t + 1else:break

3.2.5 轨迹图

plt.plot(traj_t[:41], traj_x[0][:41], '-bo', label='x(1)')
plt.plot(traj_t[:41], traj_x[1][:41], '-.go', label='x(2)')
plt.plot(traj_t[:41], traj_x[2][:41], '--ro', label='x(3)')
plt.plot(traj_t[:41], traj_x[3][:41], '-co', label='x(4)')
plt.legend(loc='best')
plt.show()

考虑在该控制更新策略下，系统状态在前两秒的轨迹图为：

通过对比，可以发现与文章中提供的图基本相同。

4. 发现的问题

通过将近一个礼拜的研究，我发现了具体实现本文算法时还存在一些问题：

问题1：实际计算的P无法收敛于论文中仿真部分给出的P∗P^*P∗值

我的思考：造成这一根本原因就是算法中采用了（10），通过微分方程数值解来近似积分，造成了最小二乘法估算P时，总是存在误差，而这个误差经过几次迭代放大后，就会造成P不收敛。而要完全复现论文中的数据，就必须保证给critic_lstsq(deltabx, deltaV)提供的deltaV必须是完全满足（11）式。换句话说，就是如果考虑从环境接收到的d(xˉ,ki)d(\bar{x}, k_i)d(xˉ,ki)是完全精确的，那么就可以通过最小二乘法计算出唯一的精确解P，满足Lyapunov方程，最终多次迭代收敛到最优值, 如下图

|p_i - p_{i-1}|: 1.0460854795299647e-05
lyap p is:[[0.4599705  0.69112794 0.05194142 0.464249  ][0.69112794 1.86677973 0.20019781 0.57995739][0.05194142 0.20019781 0.05331511 0.03015533][0.464249   0.57995739 0.03015533 2.21057234]]
calc P is:[[0.45997191 0.69112909 0.05194147 0.46425062][0.69112909 1.86678649 0.20019865 0.57995827][0.05194147 0.20019865 0.05331522 0.03015533][0.46425062 0.57995827 0.03015533 2.21057422]]

因此文章中的仿真结果，值得怀疑是否是通过这种取巧的方法而不是通过本身算法要求的附加系统求微分方程来取得。

问题2：误差ϵ\epsilonϵ的取值对与算法是否收敛影响很大

在相同初始参数下，如果将误差取为ϵ=0.01\epsilon=0.01ϵ=0.01, 那么通过计算，P可以收敛；

|p_i -P_{i-1}|: 0.008006732152754513
lyap p is:[[0.45997193 0.6911291  0.05194148 0.46425065][0.6911291  1.86678652 0.20019866 0.57995828][0.05194148 0.20019866 0.05331522 0.03015533][0.46425065 0.57995828 0.03015533 2.21057427]]
calc P is:[[0.46098944 0.69194825 0.05197939 0.46541675][0.69194825 1.87195893 0.20081381 0.58055623][0.05197939 0.20081381 0.05339299 0.03015332][0.46541675 0.58055623 0.03015332 2.21193659]]

当ϵ=0.001\epsilon=0.001ϵ=0.001时，相同条件下P最终会不满足正定阵的条件，导致系统状态在不断的错误策略迭代中导致系统发散。

|p_i -P_{i-1}|：3.5830071212353114
lyap p is:[[0.4599728  0.69112831 0.05194173 0.46425059][0.69112831 1.86678715 0.20019774 0.57995734][0.05194173 0.20019774 0.05331532 0.03015551][0.46425059 0.57995734 0.03015551 2.21057409]]
calc P is:[[ 0.68940744  1.57865928  0.05172892  0.66838222][ 1.57865928  4.14317881  0.74308701  1.44436328][ 0.05172892  0.74308701 -0.20910912 -0.00497367][ 0.66838222  1.44436328 -0.00497367  2.38735624]]

造成这一问题的原因目前还不清楚，我的猜测是之前的更新策略使得状态迅速收敛到很小的值，可能导致XXTXX^TXXT不满足广义可逆条件，因而采用最小二乘法时，就会出现错误的P，如上图中的calc P一样。因此，在真正采用该算法时，需要有一个假设条件，即最小二乘法评估的策略如果P不满足正定阵，则不执行策略更新，继续采用先前的策略。这样可以保证系统状态至少不发散，能够收敛到平衡点。

5 结语

通过一个礼拜对文章的理解加编程实现，中间一度考虑到是否是精度问题导致无法完全实现P收敛，还采用了matlab编程，但结果都显示不是精度问题导致的P有时不收敛。该算法能够作为自适应算法的一种补充，但我认为并没有如文章中写的那么好用，可以完全不考虑A。这个算法真正能用的前提就是一定要获取环境中的真实积分价值函数的值（这个不可能）还有就是确定好结束策略循环迭代的收敛误差ϵ\epsilonϵ。
后续将继续追寻这条线，往下找一下，Lewis团队是否改进了该不太好实现的方法（大概率改进了，因为这篇文章是2009年的，现在都2022年了，都采用神经网络逼近了）。如复现成功，也会作为自己的总结发表出来。