模型预测控制MPC（单输入单输出例子）

模型预测控制MPC（单输入单输出例子）

作为初学者，这篇主要记录自己学习MPC，并用python简单实现个单输入单输出的例子。
有什么不对的地方欢迎留言讨论。

模型预测控制

这一节不会讲模型预测控制的公式推导，都是默认理解模型预测控制理论的。

单输入单输出模型

单输入单输出的系统结构图如下图所示：

其中，
$r(t)$是给定值；
$e(t)$是误差，其公式为：$e=r(t)-y(t)$，这个误差也是系统的状态；
$u(t)$是系统的输入。

我们假设动态系统的行为由下面的微分方程所描述：
$\dot{x}(t)=ax(t)+bu(t)$
其中，
$x(t)$是系统状态；
$u(t)$是控制输入；
$a$、$b$是系统的常数。
代价函数可以写成如下：
$J={\int }_0^{T}q{e}_t^2+p{u}_t^{2}dt$
其中，
$q$和$p$是权重因子，分别平衡状态和控制输入的成本；
$T$是终端时间。
因为计算机无法处理连续的函数，所以我们需要将积分写成累加形式：
$x_{k+1}=a{x}_k+b{u}_k$
$J={\sum }_k^{N-1}[q{e}_k^2+p{u}_k^2]+h{e}_N^2$
其中，$N$是预测的步数，$h$是终端状态的权重。

代码实现

导入的库函数

import casadi as ca
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from matplotlib.offsetbox import OffsetImage, AnnotationBbox
import matplotlib.patches as patches
from matplotlib.ticker import MultipleLocator

建立模型

下面的函数是用于画图：

def draw(y_nums, y_ref, x_min=0, x_max=100, x_num=100):
    # 8 * 6 英寸的画布大小，每英寸有 80 个点
    fig  = plt.figure(figsize=(8,6), dpi=80)
    y1 = np.full(x_num, y_ref)
    # 设置一维坐标系
    ## 获取当前的轴对象
    ax = plt.gca()
    ## 设置x、y轴的上下限
    ax.set_xlim(-5, x_max+int(x_max/10))
    #ax.set_ylim(-8, 8)
    ## 框的上、右、左设置无颜色
    # ax.spines['right'].set_color('none')
    # ax.spines['top'].set_color('none')
    # ## 框的下原点坐标。这样就成一维坐标系
    # ax.xaxis.set_ticks_position('bottom')
    # ax.spines['bottom'].set_position(('data',0))
    # ax.yaxis.set_ticks_position('left')
    # ax.spines['left'].set_position(('data',0))
    # ## 隐藏y轴刻度
    # ax.yaxis.set_visible(False) 
    x = np.linspace(x_min, x_max, x_num, endpoint=True)    
    plt.plot(x, y_nums, color="blue", linewidth=2.0, linestyle="-")
    plt.plot(x, y1, color='red', linestyle='-', linewidth=1)
    #plt.plot(ax, x_ref, color="green", linewidth=1.0, linestyle="-")

    plt.show()    

下面的函数就是单输入单输出的建模：

def MPC_SISO():
    '''
    x_next = a * x(now) + b * u(now)
    偏差: e = x(now) - x(ref)
    预测步数: N
    J(N) = J(0) + J(1) + ... + J(N-1)
    J(0) = 0
    J(1) = J(0) + q * (e**2) + p * (u**2)
    .
    .
    .
    J(N-1) = J(N-2) + q * (e**2) + p * (u**2)
    J(N) = J(N-1) + q * (e**2) + p * (u**2)
    J = J(N) + h * (e**2)
    最终的

    假设起点到终点有 100 步，每一步都要做模型预测控制计算，每一步计算出来只取第一个数据
    '''
    # 定义系统系数 a、b
    a = 1
    b = 1
    # 定义系统最终状态 x_ref
    x_ref = 1
    # 定义预测区间 N
    N = 5
    # 定义系统的状态和输入，状态要比输入多一个
    X = ca.SX.sym("X", N+1)
    U = ca.SX.sym("U", N)
    # 定义 q(状态比重)、r（输入比重）
    q = 0.01
    p = 0.9
    h = 0.01
    # 定义初始状态 X_0
    X_0 = ca.SX.sym("X_0")
    # 定义约束，也是我认为的偏差，这是一个集合，表示预测的每一个点的偏差
    constraints = []
    constraints.append(X[0]-X_0)

    # 定义和计算 代价函数 
    J = 0
    for k in range(N):
        e = X[k] - x_ref
        J = J + q * (e**2) + p * (U[k]**2)
        x_next = a * X[k] + b * U[k]
        constraints.append(x_next - X[k+1])
    J = J + h * ((X[N] - x_ref)**2)

    # 以上步骤就完成了数学建模，接下来是求解最优
    # 创建NLP求解器
    ## 'x' 是代价函数里面需要求解的参数；'f' 是代价函数；'g' 是约束函数；'p' 是定义的初始值
    nlp = {'x': ca.vertcat(X, U), 'f': J, 'g': ca.vertcat(*constraints), 'p':X_0}
    solver = ca.nlpsol('solver', 'ipopt', nlp)
    # 初始猜测和边界
    ## x0 是初始猜测值：初始猜测值为优化求解器提供了一个起点，即从哪里开始搜索解空间。对于非线性问题，不同的起点可能导致求解器收敛到不同的局部最优解。
    ## 系统状态 X 有 N+1 个，输入 U 有 N 个，所以 共有 N+N+1 个初始猜测值
    x0 = np.zeros(N + N + 1)
    ## 约束的下界和上界都设置为0：表示预测的每个点的偏差都为 0
    ## 为什么是 N+1 ? 因为有 N+1 个预测的系统状态
    lbg = np.zeros(N + 1)
    ubg = np.zeros(N + 1)
    ## 变量的上下界设置为 无穷 
    lbx = [-ca.inf] * (N + N + 1)
    ubx = [ca.inf] * (N + N + 1)

    # 定义步数，以及每一步的模型预测控制的数据
    k_step = 100
    X_step = np.zeros((N, k_step), dtype=float)
    init_value = -3.0
    for k_i in range(k_step):
        # 解决NLP
        ## 初始值设置为 -3.0
        sol = solver(x0=x0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx, p=[init_value])
        # 提取解
        optimal_values = sol['x'].full().flatten()
        optimal_states = optimal_values[:N+1]
        optimal_controls = optimal_values[N+1:]

        print("Optimized State Trajectory:\n", optimal_states[1:6])
        print("Optimized Control Inputs:\n", optimal_controls)
        
        init_value = optimal_states[1]
        X_step[:,k_i] = optimal_states[1:6]
    y = X_step[0,:]
    draw(y_nums=y,y_ref = x_ref, x_min=0,x_max=100,x_num=k_step)

调用函数

if __name__ == "__main__":
    MPC_SISO()

输出图片

下图是以上代码所实现的，红色的直线代表设定值，蓝色曲线代表系统的状态走势。可以通过更改$q$、$p$和$h$来观察曲线的走势。若$q\gg p$则该模型控制比较看重系统的状态，曲线比较陡峭；若$p\gg q$，则相反，曲线比较平滑。

总结

以上就是模型预测控制的单输入单输出的例子，可能还存在诸多不足，欢迎各位留言讨论。