自适应动态规划（二）

贝尔曼公式和离散LQR

一个离散系统
$$x(k+1)=Ax(k)+Bu(k)$$
性能指标函数
$$J(k)=\frac{1}{2}\sum _{i=k}^{\infty }(x^T(i)Qx(i)+u^T(i)Ru(i))$$
由贝尔曼方程可知
$$\begin{array}{rl}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{array}$$
假设二次型满足
$$V(x(k))=\frac{1}{2}{x}^T(k)Px(k)$$
可得
$$\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）$$
取反馈控制律$u(k)=\mu (x(k))=-Kx(k)$可得，
$$x(k+1)=(A-BK)x(k)$$
可得
$$\frac{1}{2}{x}^T(k)Px(k)=\frac{1}{2}(x^T(k)Qx(k)+x^T(k)K^{T}RKx(k))+\frac{1}{2}{x}^T(k)(A-BK{)}^{T}P(A-BK)x(k)$$
能够得到
$$(A-BK{)}^{T}P(A-BK)-P+Q+K^{T}RK=0（3）$$
离散LQR哈密顿方程
$$H(x(k),u(k))=x^T(k)Qx(k)+u^T(k)Ru(k)+(Ax(k)+Bu(k){)}^{T}P(Ax(k)+Bu(k))-x^T(k)Px(k)$$
最优控制的时
$$\frac{\partial H(x(k),u(k))}{\partial u(k)}=0$$
可以解得最优控制序列
$$u(k)=-Kx(k)=-(B^{T}PB+R{)}^{-1}{B}^{T}PAx(k)$$
将此时带入到（3）中可以得到离散黎卡提方程
$$A^{T}PA-P+Q-A^{T}PB(B^{T}PB+R{)}^{-1}{B}^{T}PA=0$$
离散LQR的贝尔曼方程等于李雅普诺夫函数，表示成黎卡提方程。

公式（1）和（2）是不关于系统参数 $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_{i+1}=f(V_i)$ 来求解这个方程，使任何初始条件 $V_0$，上述迭代都能收敛。

控制网络参数的更新

上面控制网络的最优控制 $u^{\ast }(k)$ 是通过采样取最代价函数最小的方式得到的最优控制。这里采用论文里的求导的方式进行计算。

优化的目标，找到一个参数 $\omega$ 使下式最小。
ω=arg⁡min⁡ω{xT(k)QxT(k)+u(x(k),ω)TRu(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就可以写为
$$loss=\frac{1}{2}[u(x(k),\omega {)}^{T}Ru(x(k),\omega )+(x^T(k)Q{x}^T(k)+J(x(k+1))){]}^2$$
这个优化目标当loss=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引导是相对收敛较慢的，但是每次迭代的计算量是少的。