24/8/17算法笔记 MPC算法

MPC算法，在行动前推演一下

MPC（Model Predictive Control，模型预测控制）是一种先进的控制策略，它利用未来预测模型来优化当前的控制动作。MPC的核心思想是，在每一个控制步骤中，都基于当前系统状态的测量值，通过预测模型向前看若干步，并计算出一个使未来输出最优化的控制序列。然后，只应用这个序列的第一个控制动作，并在下一个时间步重复这个过程。

MPC算法的主要特点包括：

1. 滚动时域优化：MPC在每一个控制步骤中解决一个有限时间范围内的优化问题，而不是试图直接解决整个控制任务。

2. 模型预测：利用系统模型来预测未来的行为。这个模型可以是物理模型，也可以是基于数据的模型。

3. 约束处理：MPC可以自然地处理系统的输入和输出约束，例如，限制控制输入的最大和最小值，或者状态变量的运行范围。

4. 优化目标：通常包括跟踪预定轨迹、最小化能耗、保证系统稳定性等。

5. 反馈校正：由于实际系统与模型之间存在差异，MPC通常会在每个控制周期中使用最新的测量状态来更新预测并重新优化。

MPC算法的一般步骤包括：

1. 状态测量：获取当前系统状态的实际测量值。

2. 模型预测：使用当前状态和预测模型来预测未来的状态和输出。

3. 优化问题设置：定义目标函数和约束条件，设置优化问题。

4. 求解优化问题：求解优化问题，得到最优控制序列。

5. 应用控制动作：将优化问题的解，即控制序列的第一个控制动作应用到系统上。

6. 重复过程：在下一个控制周期，重复上述步骤。

MPC算法广泛应用于化工、石油、交通、航空等众多领域，特别是在需要考虑复杂约束和多步预测的场合。然而，MPC算法的性能在很大程度上依赖于模型的准确性和优化算法的选择。随着计算能力的提升和优化算法的发展，MPC在实际工业应用中的使用越来越广泛。

import gym
#创建环境
env = gym.make('Pendulum-v1')

定义样本池对象

import numpy as np
import torch

class Pool:
    def __init__(self,limit):
        #样本池
        self.datas = []
        self.limit = limit
    def add(self,state,action,reward,next_state,over):
        if isinstance(state,np.ndarray) or isinstance(state,torch.Tensor):#检查变量 state 是否是 bo.ndarray 或者 torch.Tensor 类型
            state = state.reshape(3).tolist()
        action = float(action)
        reward = float(reward)
        
        if isinstance(next_state,np.ndarray) or isinstance(
                next_state,torch.Tensor):
            next_state = next_state.reshape(3).tolist()
        over = bool(over)
        self.datas.appen((state,action,reward,next_state,over))
        #数据上限，超出时从最古老的开始删除
        while len(self.datas)>self.limit:
            self.datas.pop(0)
        #获取一批数据样本
    def get_sample(self):

        samples = self.datas
            
        state = torch.FloatTensor([i[0] for i in samples]).reshape(-1,3)
        action = torch.FloatTensor([i[1] for i in samples]).reshape(-1,1)
        reward = torch.FloatTensor([i[2] for i in samples]).reshape(-1,1)     
        next_state = torch.FloatTensor([i[3]for i in samples]).reshape(-1,3)
        over = torch.LongTensor([i[4]for i in samples]).reshape(-1,1)
        input = torch.cat([state,action],dim=1)
        label = torch.cat([rewarrd,next_state-state],dim=1)
        return input,label
    def _len_(self):
        return len(self_datas)

初始化样本池，并添加一局游戏的数据

pool = Pool(1000000)

#初始化一局游戏的数据
def _():
    #初始化游戏
    state = env.reset()
    
    #玩到游戏结束为止
    over = False
    while not over:
        #随机一个动作
        action = env.action_space.sample()[0]
        #执行动作，得到反馈
        next_state,reward,over,_ = env.step([action])
        
        #记录数据样本
        pool.add(state,action,reward,next_state,over)
        
        #更新游戏状态，开始下一个动作
        state = next_state

定义主模型

import random
#定义主模型
class Model(torch.nn.Module):
    
    #swish激活函数
    class Swish(torch.nn.Module):
        def __init__(self):
            super().__init__()
        def forward(self,x):
            return X*torch.sigmoid(x)

主模型中的FC层

#定义一个工具层
class FCLayer(torch.nn.Module):
    def __init__(self,in_size,out_size):
        super().__init__()
        self.in_size = in_size
        #初始化参数
        std  = in_size**0.5
        std *=2
        std = 1/std
        
        weight = torch.empty(5, in_size,out_size)
        torch.nn.init.normal_(weight,mean=0.0,std=std)
        
        self.weight = torch.nn.Parameter(weighr)
        
        self.bias = torch.nn.Parameter(torch.zeros(5,1,out_size))
    def forward(self,x):
        x = torch.bmm(x,self.weight)
        x = x+self.bias
        return x

主模型初始化函数

def __init__(self):
    super().__init__()
    
    self.sequential = torch.nn.Sequential(
        self.FCLayer(4,200), #全连接层（线性层）
        self.Swish(), #激活函数层
        self.FCLayer(200,200),
        self.Swish(),
        self.FCLayer(200,200),
        self.Swish(), 
        self.FCLayer(200,200),
        self.Swish(),
        self.FCLayer(200,8),
        torch.nn.Identity(),
    )
    self.softplus = torch.nn.Softplus() #Softplus 是一个平滑的ReLU激活函数
    self.optimizer = torch.optim.Adam(self.parameters(),lr=1e-3)

主模型计算过程,计算结果是一个均值和log方差，对logvar的加减操作是为了调整logvar的最大最小值

def forward(self,x):
    x = self.sequential(x)
    mean  = x[...,:4]
    
    logvar = x[...,4:]
    
    logvar = 0.5 - logvar
    
    logva = 0.5 -self.softplus(logvar)
    
    logvar = logvar+10
    
    logvar = self.softplus(logvar)-10
    return mean,logvar

主模型的训练函数

def train(self,input,label):
    #反复训练N次
    for _ in range(len(input)//64*20):
        #从全量数据中抽样64个，反复抽5遍，形成5份数据
        select = [torch.randperm(len(input))[:64] for _ in range(5)]
        select =torch.stack(select)
        
        input_select = input[select]
        label_select = label[select]
        del select
        
        #模型计算
        mean,logvar = model(input_select)
        
        #计算loss
        mse_loss = (mean - label_select)**2*(-logvar).exp()
        
        mse_loss = mse_loss.mean(dim=1).mean()
        var_loss+logvar.mean(dim=1).mean()
        
        loss =mse_loss+var_loss
        self.optimizer.zero_grad()
        loss.backward()
        self.optimizer.step()

初始化主模型

model = Model()

a,b = model(torch.randn(5,64,4))
a.shape,b.shape

MPC fake step函数，功能是根据state和action估计reward和next_state

class MPC:
    def _fake_step(self,state,action):
        
        input = torch.cat([state,action],dim=1)
        
        #重复5遍
        input = input.unsqueeze(dim=0).repeat([5,1,1])
        
        #模型计算
        with torch.no_grad():
            mean,std = model(input)
        std = std.exp().sqrt()
        del input
        
        #means的后3列加上环境数据
        mean[:,:,1:]+=state
        
        #重采样
        sample = torch.distributions.Normal(0,1).sample(mean.shape)
        
        #0-4的值域采样b个元素
        select = [random.choice(range(5))for _ in range(mean.shape[1])]
        
        #重采样结果，第0个维度，0-4随机选择，第二个维度，0-b顺序选择
        sample = sample[select,range(mean.shape[1])]
        
        #切分一下，就成了rewards,next_state
        reward,next_state = sample[:,:1],sample[:,1:]
        return reward,next_state

MPC cem优化函数，虚拟N条动作链，并优化M次，求与孤寂结果最佳的分布

    def _cem_optimizer(self,state,mean):
        state = torch.FloatTensor(state).reshape(1,3)
        var = torch.ones(25)
        
        #当前游戏的环境信息，复制50次
        state = state.repeat(50,1)
        
        #循环5次，寻找最优解
        for _ in range(5):
            #采样50个标准正态分布数据作为action
            actions = torch.distributions.Normal(0,1).sample([50,25])
            
            #乘以标准差，加上均值
            actions *=var**0.5
            actions +=mean
            
            #计算每条动作序列的累计奖励
            
            reward_sum = torch.zeros(50,1)
            
            #遍历25个动作
            for i in range(25):
                action = actions[:,i].unsqueeze(dim=1)
                
                #现在是不能真的去玩游戏的，只能去预测reward和next_state
                reward,next_state = self._fake_step(state,action)
                
                reward_sum += reward
                
                state = next_state
            #按照reward_sum从小到大排序
            select = torch.sort(reward_sum.squeeze(dim=1)).indices
            actions = actions[select]
            del select
            
            #取发聩最优的10个动作链
            actions = actions[-5:]
            
            #在下一次随机动作时，希望贴近这些动作的分布
            new_mean = actions.mean(dim= 0)
            new_var = actions.var(dim=0)
            
            #增量更新
            mean = 0.1*mean +0.9*new_mean
            var = 0.1*var+0.9*new_var
        return mean

MPC mpc函数，每次动作都预演N次，求预演结果最佳的分布

def mpc(self):
    #初始化动作的分布均值都是0
    mean = torch.zeros(25)
    
    reward_sum = 0
    state = env.reset()
    over = False
    while not over:
        #当前状态下，找25个最优都知道均值
        actions = self._cem_optimize(state,mean)
        #执行第一个动作
        action = actions[0].item()
        
        #执行动作
        next_state,reward,over,_ = env.step([action])
        
        #增加数据
        pool.add(state,action,reward,next_state,over)
        
        state = next_state
        reward_sum +=reward
        
        #下个动作的均值，在当前动作均值的基础上寻找
        mean = torch.empty(actions.shape)
        mean[:-1] = actions[1:]
        mean[-1] = 0
    return reward_sum

初始化MPC对象

mpc = MPC()
a,b = mpc._fake_step(torch.randn(200,3),torch.randn(200,1))
print(a.shape,b.shape)
print(mpc._cem_optimize(torch.randn(1,3),,torch.zeros(25)).shape)

训练

for i in range(10):
    input,label = pool.get_sample()
    model.train(input,label)
    reward_sum = mpc.mpc()
    print(i,len(pool),reward_sum)