自适应动态规划(二)
贝尔曼公式和离散LQR
一个离散系统
x
(
k
+
1
)
=
A
x
(
k
)
+
B
u
(
k
)
x(k+1)=Ax(k)+Bu(k)
x(k+1)=Ax(k)+Bu(k)
性能指标函数
J
(
k
)
=
1
2
∑
i
=
k
∞
(
x
T
(
i
)
Q
x
(
i
)
+
u
T
(
i
)
R
u
(
i
)
)
J(k)=\frac{1}{2}\sum_{i=k}^{\infty}(x^T(i)Qx(i)+u^T(i)Ru(i))
J(k)=21i=k∑∞(xT(i)Qx(i)+uT(i)Ru(i))
由贝尔曼方程可知
V
(
x
(
k
)
)
=
1
2
∑
i
=
k
∞
(
x
T
(
i
)
Q
x
(
i
)
+
u
T
(
i
)
R
u
(
i
)
)
V
(
x
(
k
)
)
=
1
2
(
x
T
(
k
)
Q
x
(
k
)
+
u
T
(
k
)
R
u
(
k
)
)
+
V
(
x
(
k
+
1
)
)
(
1
)
\begin{aligned} V(x(k))&=\frac{1}{2}\sum_{i=k}^{\infty}(x^T(i)Qx(i)+u^T(i)Ru(i)) \\ V(x(k))&=\frac{1}{2}(x^T(k)Qx(k)+u^T(k)Ru(k))+V(x(k+1)) ~~~~~~~~~~(1) \end{aligned}
V(x(k))V(x(k))=21i=k∑∞(xT(i)Qx(i)+uT(i)Ru(i))=21(xT(k)Qx(k)+uT(k)Ru(k))+V(x(k+1)) (1)
假设二次型满足
V
(
x
(
k
)
)
=
1
2
x
T
(
k
)
P
x
(
k
)
V(x(k))=\frac{1}{2}x^T(k)Px(k)
V(x(k))=21xT(k)Px(k)
可得
1
2
x
T
(
k
)
P
x
(
k
)
=
1
2
(
x
T
(
k
)
Q
x
(
k
)
+
u
T
(
k
)
R
u
(
k
)
)
+
1
2
x
T
(
k
+
1
)
P
x
(
k
+
1
)
(
2
)
\frac{1}{2}x^T(k)Px(k)=\frac{1}{2}(x^T(k)Qx(k)+u^T(k)Ru(k))+\frac{1}{2}x^T(k+1)Px(k+1) ~~~~~~~~~~~~~~(2)
21xT(k)Px(k)=21(xT(k)Qx(k)+uT(k)Ru(k))+21xT(k+1)Px(k+1) (2)
取反馈控制律
u
(
k
)
=
μ
(
x
(
k
)
)
=
−
K
x
(
k
)
u(k)=\mu(x(k))=-Kx(k)
u(k)=μ(x(k))=−Kx(k)可得,
x
(
k
+
1
)
=
(
A
−
B
K
)
x
(
k
)
x(k+1)=(A-BK)x(k)
x(k+1)=(A−BK)x(k)
可得
1
2
x
T
(
k
)
P
x
(
k
)
=
1
2
(
x
T
(
k
)
Q
x
(
k
)
+
x
T
(
k
)
K
T
R
K
x
(
k
)
)
+
1
2
x
T
(
k
)
(
A
−
B
K
)
T
P
(
A
−
B
K
)
x
(
k
)
\frac{1}{2}x^T(k)Px(k)=\frac{1}{2}(x^T(k)Qx(k)+x^T(k)K^TRKx(k))+\frac{1}{2}x^T(k)(A-BK)^TP(A-BK)x(k)
21xT(k)Px(k)=21(xT(k)Qx(k)+xT(k)KTRKx(k))+21xT(k)(A−BK)TP(A−BK)x(k)
能够得到
(
A
−
B
K
)
T
P
(
A
−
B
K
)
−
P
+
Q
+
K
T
R
K
=
0
(
3
)
(A-BK)^TP(A-BK)-P+Q+K^TRK=0 ~~~~~~~~~~~~~~~~~~~~(3)
(A−BK)TP(A−BK)−P+Q+KTRK=0 (3)
离散LQR哈密顿方程
H
(
x
(
k
)
,
u
(
k
)
)
=
x
T
(
k
)
Q
x
(
k
)
+
u
T
(
k
)
R
u
(
k
)
+
(
A
x
(
k
)
+
B
u
(
k
)
)
T
P
(
A
x
(
k
)
+
B
u
(
k
)
)
−
x
T
(
k
)
P
x
(
k
)
H(x(k),u(k))=x^T(k)Qx(k)+u^T(k)Ru(k)+(Ax(k)+Bu(k))^TP(Ax(k)+Bu(k))-x^T(k)Px(k)
H(x(k),u(k))=xT(k)Qx(k)+uT(k)Ru(k)+(Ax(k)+Bu(k))TP(Ax(k)+Bu(k))−xT(k)Px(k)
最优控制的时
∂
H
(
x
(
k
)
,
u
(
k
)
)
∂
u
(
k
)
=
0
\frac{\partial H(x(k),u(k))}{\partial u(k)}=0
∂u(k)∂H(x(k),u(k))=0
可以解得最优控制序列
u
(
k
)
=
−
K
x
(
k
)
=
−
(
B
T
P
B
+
R
)
−
1
B
T
P
A
x
(
k
)
u(k)=-Kx(k)=-(B^TPB+R)^{-1}B^TPAx(k)
u(k)=−Kx(k)=−(BTPB+R)−1BTPAx(k)
将此时带入到(3)中可以得到离散黎卡提方程
A
T
P
A
−
P
+
Q
−
A
T
P
B
(
B
T
P
B
+
R
)
−
1
B
T
P
A
=
0
A^TPA-P+Q-A^TPB(B^TPB+R)^{-1}B^TPA=0
ATPA−P+Q−ATPB(BTPB+R)−1BTPA=0
离散LQR的贝尔曼方程等于李雅普诺夫函数,表示成黎卡提方程。
公式(1)和(2)是不关于系统参数 A , B A,B A,B 的而公式(3)就必须已知系统信息才能求解。自适应动态规划就是利用式(1)或式(2),对系统进行最优控制,而不需要知道系统参数。
参考文献:Reinforcement Learning and Feedback Control: Using Natural Decision Methods to Design Optimal Adaptive Controllers
迭代自适应动态规划基本思想
我们需要解方程
V
(
x
(
k
)
)
=
min
u
(
k
)
{
U
(
x
(
k
)
,
u
(
k
)
)
+
V
(
x
(
k
+
1
)
)
}
V(x(k))=\min_{u(k)}\{U(x(k),u(k))+V(x(k+1))\}
V(x(k))=u(k)min{U(x(k),u(k))+V(x(k+1))}
考虑方程
V
=
f
(
V
)
V=f(V)
V=f(V),可以通过迭代算法
V
i
+
1
=
f
(
V
i
)
V_{i+1}=f(V_i)
Vi+1=f(Vi) 来求解这个方程,使任何初始条件
V
0
V_0
V0,上述迭代都能收敛。
控制网络参数的更新
上面控制网络的最优控制 u ∗ ( k ) u^*(k) u∗(k) 是通过采样取最代价函数最小的方式得到的最优控制。这里采用论文里的求导的方式进行计算。
优化的目标,找到一个参数
ω
\omega
ω 使下式最小。
ω
=
arg
min
ω
{
x
T
(
k
)
Q
x
T
(
k
)
+
u
(
x
(
k
)
,
ω
)
T
R
u
(
x
(
k
)
,
ω
)
+
J
(
x
(
k
+
1
)
)
}
\omega=\arg\min_{\omega}\{x^T(k)Qx^T(k)+u(x(k),\omega)^TRu(x(k),\omega)+J(x(k+1))\}
ω=argωmin{xT(k)QxT(k)+u(x(k),ω)TRu(x(k),ω)+J(x(k+1))}
则loss就可以写为
l
o
s
s
=
1
2
[
u
(
x
(
k
)
,
ω
)
T
R
u
(
x
(
k
)
,
ω
)
+
(
x
T
(
k
)
Q
x
T
(
k
)
+
J
(
x
(
k
+
1
)
)
)
]
2
loss=\frac{1}{2}[u(x(k),\omega)^TRu(x(k),\omega)+(x^T(k)Qx^T(k)+J(x(k+1)))]^2
loss=21[u(x(k),ω)TRu(x(k),ω)+(xT(k)QxT(k)+J(x(k+1)))]2
这个优化目标当loss=0时,
J
(
k
)
=
0
J(k)=0
J(k)=0
改成这种情况下的代码
代码例子
还是抄上一家的,但是他的控制网络的loss写错了,通过我上面的分析,我的代码已经改正,可以完全收敛,并且能够达到很好的控制效果。
torch.manual_seed(1) # 设置随机种子,使得每次生成的随机数是确定的
state_dim = 2 # 状态维度
v_dim = 1 # 价值维度
action_dim = 1 # 动作维度
learing_rate = 0.0005 # 学习率
learing_num = 5000 # 学习次数
sim_num = 20 # 仿真步长
x0 = np.array([2,-1]) # 初始状态
epislon = 0.001 # 阈值
########################################################################################################################
# 定义神经网络类
########################################################################################################################
class Model(torch.nn.Module):
# 初始化
def __init__(self):
super(Model, self).__init__()
self.lay1 = torch.nn.Linear(state_dim, 10, bias = False) # 线性层
self.lay1.weight.data.normal_(0,0.5) # 权重初始化
self.lay2 = torch.nn.Linear(10, 1, bias = False) # 线性层
self.lay2.weight.data.normal_(0, 0.5) # 权重初始化
def forward(self, x):
layer1 = self.lay1(x) # 第一隐层
layer1 = torch.nn.functional.relu(layer1) # relu激活函数
output = self.lay2(layer1) # 输出层
return output
########################################################################################################################
# 定义价值迭代类
########################################################################################################################
class Value_Iteration():
def __init__(self):
self.V_model = Model() # 定义V1网络
self.A_model = Model() # 定义A网络
self.criterion = torch.nn.MSELoss(reduction='mean') # 平方误差损失
# 训练一定次数,更新Critic Net的参数
# 这里只需要定义A网络和V2网络的优化器
self.optimizerV = torch.optim.SGD(self.V_model.parameters(), lr=learing_rate) # 利用梯度下降算法优化model.parameters
self.optimizerA = torch.optim.SGD(self.A_model.parameters(), lr=learing_rate) # 利用梯度下降算法优化model.parameters
# 采样状态 将状态定义在x1 [-2,2] x2 [-1,1]
x = np.arange(-2, 2, 0.1)
y = np.arange(-1, 1, 0.1)
xx, yy = np.meshgrid(x, y) # 为一维的矩阵
self.state = np.transpose(np.array([xx.ravel(), yy.ravel()])) # 所有状态 ravel()将多维数组转换为一维数组
self.state_num = self.state.shape[0] # 状态个数
self.cost = [] # 初始化误差矩阵
####################################################################################################################
# 定义模型函数
####################################################################################################################
def model(self, current_state, u):
next_state = np.zeros([current_state.shape[0], current_state.shape[1]]) # 初始化下一个状态
for index in range(current_state.shape[0]): # 对每个样本计算下一个状态 根据输入的u
next_state[index, 0] = 0.2 * current_state[index, 0] * np.exp(current_state[index, 1] ** 2)
next_state[index, 1] = 0.3 * current_state[index, 1] ** 3 - 0.2 * u[index]
return next_state
####################################################################################################################
# J_loss函数
####################################################################################################################
def J_loss(self,sk,uk,Vk_1): #性能指标计算
Vk = np.zeros(uk.shape[0]) # x_k 的V值
for index in range(uk.shape[0]): # 对每个样本计算下一个状态 根据输入的u
Vk[index] = sk[0] ** 2 + sk[1] ** 2 + uk[index] ** 2 + Vk_1[index]
pass
return Vk
pass
####################################################################################################################
# 定义学习函数
####################################################################################################################
def learning(self):
for train_index in range(learing_num):
print('the ' , train_index+1 , ' --th learing start')
with torch.no_grad():
last_V_value = self.V_model(Variable(torch.Tensor(self.state))).data
#############################################################################################################
# 更新Actor网络
#############################################################################################################
A_predict = self.A_model( Variable(torch.Tensor(self.state))) # 预测值
la_u = A_predict
la_next_state = self.model(self.state, A_predict)
A_target = np.zeros([self.state_num, 1]) # 初始化A网络的标签
for index in range(self.state_num): # 循环计算所有状态的标签
next_V = self.V_model(torch.Tensor(la_next_state[index, :]))
A_target[index] = self.state[index, 0] ** 2 + self.state[index, 1] ** 2 + next_V.data
A_loss = self.criterion(A_predict*A_predict, Variable(torch.Tensor(-A_target))) # 计算损失
self.optimizerA.zero_grad() # 对模型参数做一个优化,并且将梯度清0
A_loss.backward() # 计算梯度
self.optimizerA.step() # 权重更新
print("A_loss:",A_loss)
#############################################################################################################
# 更新Crictic网络
#############################################################################################################
V_predict = self.V_model(Variable(torch.Tensor(self.state))) # 预测值
V_target = np.zeros([self.state_num, 1]) # 初始化V网络的标签
for index in range(self.state_num): # 循环计算所有状态的标签
next_V = self.V_model(torch.Tensor(la_next_state[index, :]))
V_target[index] = self.state[index, 0] ** 2 + self.state[index, 1] ** 2 + la_u.data[index] ** 2 + next_V.data
V_loss = self.criterion(V_predict, torch.Tensor(V_target)) # 计算损失
self.optimizerV.zero_grad() # 对模型参数做一个优化,并且将梯度清0
V_loss.backward() # 计算梯度
self.optimizerV.step() # 权重更新
print("V_loss:",V_loss)
with torch.no_grad():
V_value = self.V_model(Variable(torch.Tensor(self.state))).data # 计算V
pp = np.abs(V_value)-np.abs(last_V_value)
dis = np.sum(np.array(pp.reshape(self.state_num))) #平方差
self.cost.append(np.abs(dis)) #记录下误差
print(' deta(V): ',np.abs(dis))
if np.abs(dis) < epislon:
print('loss小于阈值,退出训练')
break
# 保存和加载整个模型
# 每次训练完可以保存模型,仿真时候 直接load训练好的模型 或者 继续训练可以接着上一次训练的结果继续训练
#torch.save(model_object, 'model.pth')
#model = torch.load('model.pth')
#################################
######################################################################
# 定义仿真函数
# 通过得到的Actor选择动作
# 同时利用Critic计算V
#######################################################################################################
def simulator(self):
print('the simulation is start')
#self.restore(self.path)
State_traject = np.zeros([sim_num + 1, state_dim])
State_traject[0, :] = x0
u_traject = np.zeros([sim_num, 1])
for index in range(sim_num):
print('the ', index, ' --th time start')
print('当前状态:', State_traject[index,:])
sim_actor = self.A_model(torch.Tensor(State_traject[index,:]))
print('当前输入:',sim_actor)
u_traject[index] = sim_actor.data
sim_nexstate = self.model(State_traject[index, :].reshape(1, 2), sim_actor.data)
print('下一时刻状态:', sim_nexstate)
State_traject[index + 1, :] = sim_nexstate
V_traject = self.V_model(torch.Tensor(State_traject)).data
print('the simulation is over')
self.plot_curve(State_traject , u_traject , V_traject , self.cost)
pass
#######################################################################################################
# 绘图函数
# 分别绘制状态轨迹 控制输入u轨迹 值函数V轨迹
# 并将结果保存!
#######################################################################################################
def plot_curve(self, s, u, V,cost):
# print('\nstate\n',s)
# print('\nu\n', u)
# print('\nV\n', V)
# 绘制状态轨迹
plt.figure(1)
plt.plot(s[:, 0], 'r', label='State_1')
plt.plot(s[:, 1], 'b', label='State_2')
plt.title('State_Trajecteory')
plt.xlabel('iter')
plt.ylabel('State')
plt.legend()
plt.grid()
plt.savefig(r'ADPresultfig\HDP_with_targetnet_state.png')
plt.show()
# 绘制控制输入u轨迹
plt.figure(2)
plt.plot(u, )
plt.title('U_Trajecteory')
plt.xlabel('iter')
plt.ylabel('u')
plt.grid()
plt.savefig(r'ADPresultfig\HDP_with_targetnet_u.png')
plt.show()
# 绘制值函数V的轨迹
plt.figure(3)
plt.plot(V, 'r')
plt.title('Cost_Trajecteory')
plt.xlabel('iter')
plt.ylabel('Cost')
plt.grid()
plt.savefig(r'ADPresultfig\HDP_with_targetnet_V.png')
plt.show()
# 绘制值函数V的轨迹
plt.figure(4)
plt.plot(cost, 'r')
plt.title('Train_loss_Trajecteory')
plt.xlabel('iter')
plt.ylabel('Train_loss')
plt.grid()
plt.savefig(r'ADPresultfig\HDP_with_targetnet_loss.png')
plt.show()
pass
########################################################################################################################
# 函数起始运行
# 在仿真时候,直接调用最优的模型进行仿真
# 最优的模型根据损失函数进行判断
########################################################################################################################
if __name__ == '__main__':
Agent = Value_Iteration() # 值迭代类实例化 使评价网络和控制网络学习
Agent.learning() # 学习
Agent.simulator()
由于代码中,将两个评价网络变成了一个评价网络,网络的收敛性收到了很大的影响,而且用采样的方法找到的一定是最小代价的控制,而用loss引导是相对收敛较慢的,但是每次迭代的计算量是少的。