do-MPC:Getting started: MPC

Getting Start do-mpc

原文地址do-mpc
我们从导入所需的模块开始，最显著的是do_mpc.
import numpy as np

# Add do_mpc to path. This is not necessary if it was installed via pip.
# 将do_mpc添加到路径。如果是通过pip安装的，则没有必要这样做。
import sys
import os
rel_do_mpc_path = os.path.join('..','..')
sys.path.append(rel_do_mpc_path)

# Import do_mpc package:
import do_mpc

do mpc的一个基本范式是模块化架构，根据应用程序的不同，单个积木可以独立使用，也可以联合使用。
在下文中，我们将从model开始介绍这些块之间的配置、设置和连接

示例系统

首先，我们介绍了一个简单的系统，我们为它设置了do-mpc。我们希望控制三重质量弹簧系统，如下所示：

三个旋转圆盘通过弹簧连接，我们将它们的角度表示为${\varphi }_1,{\varphi }_2,{\varphi }_3$,最外面的两个圆盘分别连接到带有附加弹簧的步进电机。步进电机角度${\varphi }_{m,1}$和${\varphi }_{m,2}$被用作系统的输入。该系统的相关参数是三个圆盘的惯性$\Theta$、弹簧常数$c$以及阻尼系数$d$。
这个系统的二阶ODE可以写如下：
$$\begin{array}{l}{\Theta }_1{\ddot{\varphi }}_1=-c_1\left({\varphi }_1-{\varphi }_{m,1}\right)-c_2\left({\varphi }_1-{\varphi }_2\right)-d_1{\dot{\varphi }}_1\\ {\Theta }_2{\ddot{\varphi }}_2=-c_2\left({\varphi }_2-{\varphi }_1\right)-c_3\left({\varphi }_2-{\varphi }_3\right)-d_2{\dot{\varphi }}_2\\ {\Theta }_3{\ddot{\varphi }}_3=-c_3\left({\varphi }_3-{\varphi }_2\right)-c_4\left({\varphi }_3-{\varphi }_{m,2}\right)-d_3{\dot{\varphi }}_3\end{array}$$
从非零初始状态开始的非受控系统将持续一段较长的时间，如下所示：

稍后，我们希望能够有效地使用电机，使振荡质量停止。它看起来像这样：

创建模型

如上所述，model对于do-mpc的应用是必不可少的。在数学术语中，该模型被定义为连续常微分方程（ODE）、微分代数方程（DAE）或离散方程。
在DAE/ODE的情况下，我们写道：
$$\begin{array}{rl}\frac{\partial x}{\partial t}& =f(x,u,z,p)\\ 0& =g(x,u,z,p)\\ y& =h(x,u,z,p)\end{array}$$
我们将$x\in {\mathbb{R}}^{n_x}$表示为状态，$u\in {\mathbb{R}}^{n_u}$表示为输入，$z\in {\mathbb{R}}^{n_z}$表示代数状态，$p\in {\mathbb{R}}^{n_p}$表示参数。
我们将上面的二阶常微分方程重新表述为以下的一阶常微分函数，引入以下状态：
$$\begin{array}{l}{x}_1={\varphi }_1\\ x_2={\varphi }_2\\ x_3={\varphi }_3\\ x_4={\dot{\varphi }}_1\\ x_5={\dot{\varphi }}_2\\ x_6={\dot{\varphi }}_3\end{array}$$
并导出右手边函数 $f(x,u,z,p)$ 为：
$$\begin{array}{l}{\dot{x}}_1=x_4\\ {\dot{x}}_2=x_5\\ {\dot{x}}_3=x_6\\ {\dot{x}}_4=-\frac{c_1}{{\Theta }_1}\left(x_1-u_1\right)-\frac{c_2}{{\Theta }_1}\left(x1-x_2\right)-\frac{d_1}{{\Theta }_1}{x}_4\\ {\dot{x}}_5=-\frac{c_2}{{\Theta }_2}\left(x_2-x_1\right)-\frac{c_3}{{\Theta }_2}\left(x_2-x_3\right)-\frac{d_2}{{\Theta }_2}{x}_5\\ {\dot{x}}_6=-\frac{c_3}{{\Theta }_3}\left(x_3-x_2\right)-\frac{c_4}{{\Theta }_3}\left(x_4-u_2\right)-\frac{d_3}{{\Theta }_3}{x}_6\end{array}$$
有了这个理论背景，我们就可以开始配置do-mpc的model对象了。
首先，我们需要决定模型类型。对于给定的示例，我们使用的是一个连续模型。

model_type = 'continuous' # either 'discrete' or 'continuous'
model = do_mpc.model.Model(model_type)

模型变量

下一步是定义模型变量。定义变量类型、名称以及可选的shape（默认为标量）非常重要。以下类型可用：

Long name	short name	Remark
states	_x	Required
inputs	_u	Required
algebraic	_z	Optional
parameter	_p	Optional
timevarying_parameter	_tvp	Optional

phi_1 = model.set_variable(var_type='_x', var_name='phi_1', shape=(1,1))
phi_2 = model.set_variable(var_type='_x', var_name='phi_2', shape=(1,1))
phi_3 = model.set_variable(var_type='_x', var_name='phi_3', shape=(1,1))
# Variables can also be vectors:
# 变量也可以是矢量：
dphi = model.set_variable(var_type='_x', var_name='dphi', shape=(3,1))
# Two states for the desired (set) motor position:
# 所需（设定）电机位置的两种状态：
phi_m_1_set = model.set_variable(var_type='_u', var_name='phi_m_1_set')
phi_m_2_set = model.set_variable(var_type='_u', var_name='phi_m_2_set')
# Two additional states for the true motor position:
# 真实电机位置的两种附加状态：
phi_1_m = model.set_variable(var_type='_x', var_name='phi_1_m', shape=(1,1))
phi_2_m = model.set_variable(var_type='_x', var_name='phi_2_m', shape=(1,1))

请注意，model.set_variable()返回符号变量：

print('phi_1={}, with phi_1.shape={}'.format(phi_1, phi_1.shape))
print('dphi={}, with dphi.shape={}'.format(dphi, dphi.shape))

phi_1=phi_1, with phi_1.shape=(1, 1)
dphi=[dphi_0, dphi_1, dphi_2], with dphi.shape=(3, 1)

查询变量

如果在任何时候您需要获取模型变量，例如，如果您在与其他do mpc模块不同的文件中创建模型，则可能需要检索定义的变量。do-mpc通过model属性x、u、z、p、tvp、y和aux来促进这一过程：

model.x

<casadi.tools.structure3.ssymStruct at 0x7fa718a27d30>
属性本身是一个结构化的符号变量，包含用户定义的变量。这些可以通过索引访问：

model.x['phi_1']

SX(phi_1)
请注意，这与上面model.set_variable的输出相同：

bool(model.x['phi_1'] == phi_1)

True
在具有多个元素的变量的情况下，可以使用其他索引：

model.x['dphi',0]

SX(dphi_0)
为了从符号结构中获得更多信息：

model.x.keys()

[‘phi_1’, ‘phi_2’, ‘phi_3’, ‘dphi’, ‘phi_1_m’, ‘phi_2_m’]

model.x.labels()

[‘[phi_1,0]’,
‘[phi_2,0]’,
‘[phi_3,0]’,
‘[dphi,0]’,
‘[dphi,1]’,
‘[dphi,2]’,
‘[phi_1_m,0]’,
‘[phi_2_m,0]’]

模型参数

接下来我们定义参数。已知值可以也应该是硬编码(hardcoded)的，但考虑到鲁棒的MPC，我们明确地定义了不确定的参数。我们假设惯性是这样一个不确定的参数，并硬编码弹簧常数和摩擦系数。

# As shown in the table above, we can use Long names or short names for the variable type.
# 如上表所示，我们可以为变量类型使用长名称或短名称。
Theta_1 = model.set_variable('parameter', 'Theta_1')
Theta_2 = model.set_variable('parameter', 'Theta_2')
Theta_3 = model.set_variable('parameter', 'Theta_3')

c = np.array([2.697,  2.66,  3.05, 2.86])*1e-3
d = np.array([6.78,  8.01,  8.82])*1e-5

右侧方程式 (Right-hand-side equation)

最后，我们通过调用model.set_rhs(var_name，expr)和上面定义的状态变量中的var_name以及$x,u,z,p$的表达式来设置模型的右侧。

model.set_rhs('phi_1', dphi[0])
model.set_rhs('phi_2', dphi[1])
model.set_rhs('phi_3', dphi[2])

对于向量值状态dphi，我们需要连接符号表达式。我们导入符号库CasADi：

from casadi import *
dphi_next = vertcat(
    -c[0]/Theta_1*(phi_1-phi_1_m)-c[1]/Theta_1*(phi_1-phi_2)-d[0]/Theta_1*dphi[0],
    -c[1]/Theta_2*(phi_2-phi_1)-c[2]/Theta_2*(phi_2-phi_3)-d[1]/Theta_2*dphi[1],
    -c[2]/Theta_3*(phi_3-phi_2)-c[3]/Theta_3*(phi_3-phi_2_m)-d[2]/Theta_3*dphi[2],
)

model.set_rhs('dphi', dphi_next)
tau = 1e-2
model.set_rhs('phi_1_m', 1/tau*(phi_m_1_set - phi_1_m))
model.set_rhs('phi_2_m', 1/tau*(phi_m_2_set - phi_2_m))

通过调用model.setup()来完成模型设置：

model.setup()

调用model.setup()后，我们无法定义更多的变量等。

配置MPC控制器

通过配置和设置模型，我们现在可以创建用于模型预测控制（MPC）的优化器。我们从创建对象开始（将model作为唯一的输入）

mpc = do_mpc.controller.MPC(model)

优化参数

接下来，我们需要对优化器进行参数化。有关可用参数及其含义的完整描述，请参阅optimizer.set_param()的API文档。许多参数已经具有建议的默认值。最重要的是，我们需要设置n_horizon和t_step。对于本例，我们还选择n_robust=1，默认值为0
注意，默认情况下，连续系统是通过配置离散化的

setup_mpc = {
    'n_horizon': 20,
    't_step': 0.1,
    'n_robust': 1,
    'store_full_solution': True,
}
mpc.set_param(**setup_mpc)

目标函数

MPC公式的核心是一个优化问题，我们需要为其定义一个目标函数：
$$C=\sum _{k=0}^{n-1}(\underset{\text{lagrange term }}{\underbrace{l\left(x_k,u_k,z_k,p\right)}}+\underset{\text{r-term }}{\underbrace{\Delta u_k^{T}R\Delta u_k}})+\underset{\text{meyer term }}{\underbrace{m\left(x_n\right)}}$$
我们需要定义迈耶项（mterm）和拉格朗日项（lterm）。对于给定的示例，我们设置：
$$\begin{array}{rl}l\left(x_k,u_k,z_k,p\right)& ={\varphi }_1^2+{\varphi }_2^2+{\varphi }_3^2\\ m\left(x_n\right)& ={\varphi }_1^2+{\varphi }_2^2+{\varphi }_3^2\end{array}$$

mterm = phi_1**2 + phi_2**2 + phi_3**2
lterm = phi_1**2 + phi_2**2 + phi_3**2

mpc.set_objective(mterm=mterm, lterm=lterm)

目标函数的一部分也是对控制输入的惩罚。这种惩罚通常可以用于平滑所获得的最优解，并且是一个重要的调整参数。我们对变化添加了二次惩罚：
$$\Delta u_k=u_k-u_{k-1}$$
并自动为求解器提供$\Delta u_0$的$u_{k-1}$的先前解
用户可以设置这些二次项的调谐因子，如下所示：

mpc.set_rterm(
    phi_m_1_set=1e-2,
    phi_m_2_set=1e-2
)

其中关键字参数指的是先前定义的输入名称。请注意，在上面的符号（$\Delta u_k^{T}R\Delta u_k$）中，这导致设置$R$的对角元素

约束

能够对输入和状态设置约束是MPC的一个重要特征。在do-mpc中，这些约束设置如下：

# Lower bounds on states:
mpc.bounds['lower','_x', 'phi_1'] = -2*np.pi
mpc.bounds['lower','_x', 'phi_2'] = -2*np.pi
mpc.bounds['lower','_x', 'phi_3'] = -2*np.pi
# Upper bounds on states
mpc.bounds['upper','_x', 'phi_1'] = 2*np.pi
mpc.bounds['upper','_x', 'phi_2'] = 2*np.pi
mpc.bounds['upper','_x', 'phi_3'] = 2*np.pi

# Lower bounds on inputs:
mpc.bounds['lower','_u', 'phi_m_1_set'] = -2*np.pi
mpc.bounds['lower','_u', 'phi_m_2_set'] = -2*np.pi
# Lower bounds on inputs:
mpc.bounds['upper','_u', 'phi_m_1_set'] = 2*np.pi
mpc.bounds['upper','_u', 'phi_m_2_set'] = 2*np.pi

缩放

如果OCP条件较差，例如不同的状态具有显著不同的幅度，则缩放是一个重要特征。在这种情况下，未缩放的问题可能不会导致（期望的）解决方案。可以为所有状态、输入和代数变量引入缩放因子，目标是将它们缩放到大致相同的数量级。对于给定的问题，这不是必要的，但我们简要地展示了语法（注意，也可以跳过此步骤）。

mpc.scaling['_x', 'phi_1'] = 2
mpc.scaling['_x', 'phi_2'] = 2
mpc.scaling['_x', 'phi_3'] = 2

不确定参数

do-mpc的一个重要特性是基于场景的鲁棒mpc。我们不是预测和控制单个未来轨迹，而是根据不同的不确定参数研究多个可能的轨迹。这些参数先前已在模型中定义（质量惯性）。现在，我们必须为优化器提供不同的可能场景。
这可以通过以下方式实现：

inertia_mass_1 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_2 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_3 = 2.25*1e-4*np.array([1.])

mpc.set_uncertainty_values(
    Theta_1 = inertia_mass_1,
    Theta_2 = inertia_mass_2,
    Theta_3 = inertia_mass_3
)

我们为方法优化器提供了许多关键字参数set_uncertain_parameter()。对于每个引用的参数，其值都是一个numpy.ndarray，其中包含一些可能的值。第一个值是标称情况，进一步的值将导致越来越多的场景。由于我们研究了每个可能参数的组合，因此场景的数量正在迅速增长。因此，对于我们的例子，我们仅将质量1和2的惯性视为不确定的，并且仅为惯性3的质量提供一个可能的值。

setup

配置优化器的最后一步是调用optimizer.setup，它完成设置并产生优化问题。只有现在，我们才能使用优化器来获得控制输入。

mpc.setup()

Configuring the Simulator

在许多情况下，开发的控制方法首先在模拟系统上进行测试。dompc通过dompc.simulator类来响应这种需求。模拟器使用最先进的DAE求解器，例如Sundials CVODE来求解所提供的do_mpc model中定义的DAE方程。这通常与为优化器定义的模型相同，但也可以使用同一系统的更复杂模型。
在本节中，我们将演示如何为给定的示例设置模拟器类。我们使用前面定义的model初始化类：

simulator = do_mpc.simulator.Simulator(model)

模拟器参数

接下来，我们需要对模拟器进行参数化。有关可用参数及其含义的完整描述，请参阅simulator.set_param()的API文档。许多参数已经具有建议的默认值。最重要的是，我们需要设置t_step。我们选择与优化器相同的值。

# Instead of supplying a dict with the splat operator (**), as with the optimizer.set_param(),
# we can also use keywords (and call the method multiple times, if necessary):
# 不像optimizer.set_param（）那样为dict提供splat运算符（**），
# 我们还可以使用关键字（如果需要，可以多次调用该方法）：
simulator.set_param(t_step = 0.1)

不确定参数

在模型中，我们将质量的惯性定义为参数，为此我们在优化器中选择了多个场景。模拟器现在被参数化，以在每个时间步长使用“真实”值进行模拟。在最常见的情况下，这些值可能会发生变化，这就是为什么我们需要提供一个可以在每次评估的函数来获得当前值。do mpc要求此函数具有特定的返回结构，我们首先通过调用获得该返回结构：

p_template = simulator.get_p_template()

此对象是一个CasADi结构：

type(p_template)

casadi.tools.structure3.DMStruct
其可以用以下键进行索引：

p_template.keys()

[‘default’, ‘Theta_1’, ‘Theta_2’, ‘Theta_3’]
我们现在需要编写一个函数，该函数返回具有所需数值的结构。对于我们的简单案例：

def p_fun(t_now):
    p_template['Theta_1'] = 2.25e-4
    p_template['Theta_2'] = 2.25e-4
    p_template['Theta_3'] = 2.25e-4
    return p_template

该功能现在以以下方式提供给模拟器：

simulator.set_p_fun(p_fun)

setup

与优化器类似，我们需要调用simulator.setup()来完成模拟器的设置。

simulator.setup()

创建控制回路

理论上，我们现在也可以创建一个估计器，但对于这个简明的例子，我们只假设直接状态反馈。这意味着我们现在已经准备好设置和运行控制循环了。控制循环包括使用当前状态运行优化器($x_0$)以获得电流控制输入($u_0$)然后用电流控制输入运行模拟器($u_0$)以获得下一个状态。
如前所述，我们设置了一个用于调节三质量弹簧系统的控制器。为了显示一些有趣的控制动作，我们选择了一个任意的初始状态$x_0\ne 0$：

x0 = np.pi*np.array([1, 1, -1.5, 1, -1, 1, 0, 0]).reshape(-1,1)

并使用x0属性来设置初始状态。

simulator.x0 = x0
mpc.x0 = x0

虽然我们只能设置一个常规的numpy数组，但这会填充从模型继承的状态结构：

mpc.x0

<casadi.tools.structure3.DMStruct at 0x7fa71b5ee390>
因此，我们可以通过调用：

mpc.x0['phi_1']

DM(3.14159)
请注意，属性x0（以及u0、z0和t0）始终显示类中当前变量的值。
为了设置MPC优化问题的初始猜测，我们称之为：

mpc.set_initial_guess()

所选择的初始猜测基于为MPC序列的每个元素设置的x0、z0和u0。

设置图形

为了研究控制器性能和MPC预测，我们使用了do MPC graphics。这个多功能工具使我们能够方便地基于Matplotlib配置用户定义的绘图，并可视化存储在mpc.data、simulator.data（以及如果适用的话estimator.data）对象中的结果。
我们从导入matplotlib开始：

import matplotlib.pyplot as plt
import matplotlib as mpl
# Customizing Matplotlib:
mpl.rcParams['font.size'] = 18
mpl.rcParams['lines.linewidth'] = 3
mpl.rcParams['axes.grid'] = True

并使用感兴趣的数据对象初始化图形模块。在这个特定的例子中，我们希望可视化mpc.data和simulator.data。

mpc_graphics = do_mpc.graphics.Graphics(mpc.data)
sim_graphics = do_mpc.graphics.Graphics(simulator.data)

接下来，我们创建一个figure并获取其axis对象。Matplotlib提供了多种获取轴对象的替代方法，例如 subplots, subplot2grid,或简单的gca。我们使用subplots：

%%capture
# We just want to create the plot and not show it right now. This "inline magic" supresses the output.
# 我们只是想创造图表，而不是现在就展示。这种“内联魔法”支持输出。
fig, ax = plt.subplots(2, sharex=True, figsize=(16,9))
fig.align_ylabels()

设置图形模块最重要的API元素是graphics.add_line，它模拟model.add_variable的API，但我们还需要传递一个轴。
我们希望在同一轴上显示模拟器和MPC结果，这就是为什么我们将两者配置为相同的原因：

%%capture
for g in [sim_graphics, mpc_graphics]:
    # Plot the angle positions (phi_1, phi_2, phi_2) on the first axis:
    # 绘制第一轴上的角度位置（phi_1、phi_2、phi_2）：
    g.add_line(var_type='_x', var_name='phi_1', axis=ax[0])
    g.add_line(var_type='_x', var_name='phi_2', axis=ax[0])
    g.add_line(var_type='_x', var_name='phi_3', axis=ax[0])

    # Plot the set motor positions (phi_m_1_set, phi_m_2_set) on the second axis:
    # 在第二个轴上绘制设定的电机位置（phi_m_1_set，phi_m_2_set）：
    g.add_line(var_type='_u', var_name='phi_m_1_set', axis=ax[1])
    g.add_line(var_type='_u', var_name='phi_m_2_set', axis=ax[1])


ax[0].set_ylabel('angle position [rad]')
ax[1].set_ylabel('motor angle [rad]')
ax[1].set_xlabel('time [s]')

运行模拟器

我们通过模拟没有控制输入的自主系统($u=0$)，开始研究do mpc模拟器和graphics包。这可以按如下方式进行：

u0 = np.zeros((2,1))
for i in range(200):
    simulator.make_step(u0)

我们可以使用预定义的图形来可视化生成的轨迹：

sim_graphics.plot_results()
# Reset the limits on all axes in graphic to show the data.
sim_graphics.reset_axes()
# Show the figure:
fig

根据需要，电机角度（输入）恒定为零，振荡质量慢慢停止。我们的控制目标是显著缩短制动盘静止的时间。

还记得你在上面看到的不受控制的系统的动画吗？这就是数据的来源。

运行优化器

为了获得当前控制输入，我们使用当前状态($x_0$)调用optimizer.make_step(x0)以下为：

u0 = mpc.make_step(x0)

注意，我们获得了关于给定最优控制问题（OCP）的IPOPT的输出。最重要的是，我们获得了最优解。
我们还可以通过配置图形实例来可视化预测的轨迹。首先，我们通过调用清除模拟器中的现有线路：

sim_graphics.clear()

最后，我们可以调用plot_predictions来获得：

mpc_graphics.plot_predictions()
mpc_graphics.reset_axes()
# Show the figure:
fig

我们看到了状态和最优控制输入的预测轨迹。请注意，对于三个质量的配置不确定惯性，我们看到了不同的场景。
我们还可以看到，该解决方案考虑了定义的上限和下限。对于输入来说尤其如此。

更改线条外观

在我们继续之前，我们可能应该稍微改进一下可视化。通过使用result_lines和pred_line属性，我们可以很容易地从图形模块中获取所有行对象：

mpc_graphics.pred_lines

<do_mpc.tools.structure.Structure at 0x7fa71b5ee860>
我们得到了一个可以方便查询的结构，如下所示：

mpc_graphics.pred_lines['_x', 'phi_1']

[<matplotlib.lines.Line2D at 0x7fa71c445828>,
<matplotlib.lines.Line2D at 0x7fa71c6e7898>,
<matplotlib.lines.Line2D at 0x7fa71c6e7978>,
<matplotlib.lines.Line2D at 0x7fa71c7023c8>,
<matplotlib.lines.Line2D at 0x7fa71c7022b0>,
<matplotlib.lines.Line2D at 0x7fa71c702630>,
<matplotlib.lines.Line2D at 0x7fa71c702978>,
<matplotlib.lines.Line2D at 0x7fa71c702d30>,
<matplotlib.lines.Line2D at 0x7fa71c702c88>]
我们获得了第一个状态的所有线路。要更改颜色，我们可以简单地：

 # Change the color for the three states:
for line_i in mpc_graphics.pred_lines['_x', 'phi_1']: line_i.set_color('#1f77b4') # blue
for line_i in mpc_graphics.pred_lines['_x', 'phi_2']: line_i.set_color('#ff7f0e') # orange
for line_i in mpc_graphics.pred_lines['_x', 'phi_3']: line_i.set_color('#2ca02c') # green
# Change the color for the two inputs:
for line_i in mpc_graphics.pred_lines['_u', 'phi_m_1_set']: line_i.set_color('#1f77b4')
for line_i in mpc_graphics.pred_lines['_u', 'phi_m_2_set']: line_i.set_color('#ff7f0e')

# Make all predictions transparent:
for line_i in mpc_graphics.pred_lines.full: line_i.set_alpha(0.2)

请注意，我们可以用同样的方式处理result_lines属性。例如，我们可以使用它来创建图例：

# Get line objects (note sum of lists creates a concatenated list)
lines = sim_graphics.result_lines['_x', 'phi_1']+sim_graphics.result_lines['_x', 'phi_2']+sim_graphics.result_lines['_x', 'phi_3']

ax[0].legend(lines,'123',title='disc')

# also set legend for second subplot:
lines = sim_graphics.result_lines['_u', 'phi_m_1_set']+sim_graphics.result_lines['_u', 'phi_m_2_set']
ax[1].legend(lines,'12',title='motor')

<matplotlib.legend.Legend at 0x7fa71c712eb8>

运行控制回路

最后，我们现在能够运行如上所述的控制循环。我们从优化器获得输入，然后运行模拟器。
为了确保我们重新开始，我们删除历史记录并设置模拟器的初始状态：

simulator.reset_history()
simulator.x0 = x0
mpc.reset_history()

这是主循环。我们跑了20步，这与预测范围相同。请注意，我们再次使用“捕获”来抑制IPOPT的输出。
通常建议显示输出，因为它包含有关解算器状态的重要信息。

%%capture
for i in range(20):
    u0 = mpc.make_step(x0)
    x0 = simulator.make_step(u0)

我们现在可以绘制之前显示的从时间$t=0$的预测，以及来自模拟器的闭环轨迹：

# Plot predictions from t=0
mpc_graphics.plot_predictions(t_ind=0)
# Plot results until current time
sim_graphics.plot_results()
sim_graphics.reset_axes()
fig

具有参数标称值的模拟轨迹几乎完全遵循标称开环预测。模拟轨迹由进一步的不确定场景从上到下限定。

数据处理

保存和检索结果

do-mpc数据模块中的saveresults和loadresults方法可以存储和检索do-mpc结果。我们从导入以下方法开始：

from do_mpc.data import save_results, load_results

方法save_results传递了我们想要存储的do mpc对象的列表。在我们的案例中，优化器和模拟器是可用的，并且已配置。
请注意，默认情况下，结果存储在名为results.pkl的子文件夹results中。两者都可以更改，如果文件夹还不存在，则会创建该文件夹。

save_results([mpc, simulator])

我们调查新创建的文件夹的内容：

!ls ./results/

results.pkl
save_results调用将自动检查具有给定名称的文件是否已经存在。为了避免重写，该方法在索引前加上前缀。如果我们再次保存，文件夹包含：
001_results.pkl results.pkl
pickled 结果可以通过写入以下内容手动加载：

with open(file_name, 'rb') as f:
    results = pickle.load(f)

或者使用适当的文件名（和路径）调用load_results。为了更方便起见，load_results只包含上面的代码。

results = load_results('./results/results.pkl')

获得的结果是一个字典，其中包含来自传递的do -mpc模块的数据对象。类似这样：results['optimizer']和optimizer.data包含相同的信息（类似于模拟器和估计器（如果适用））。

使用数据对象

do_mpc.data.Data对象还包含一些非常有用的属性，您应该了解这些属性。最重要的是，我们可以使用索引来查询它们，例如：

results['mpc']

<do_mpc.data.MPCData at 0x117c4df50>

x = results['mpc']['_x']
x.shape

(20, 8)
正如预期的那样，我们有20个元素（我们运行了20个步骤的循环）和8个状态。进一步的索引允许获取选定的状态：

phi_1 = results['mpc']['_x','phi_1']

phi_1.shape

(20, 1)
对于向量值状态，我们甚至可以查询：

dphi_1 = results['mpc']['_x','dphi', 0]

dphi_1.shape

(20, 1)
当然，我们也可以查询输入等。
此外，我们可以很容易地用预测方法检索预测的轨迹。语法略有不同：第一个参数是一个元组，它模仿了上面显示的索引。第二个索引是时间实例。通过以下调用，我们获得了时间0处phi_1的预测：

phi_1_pred = results['mpc'].prediction(('_x','phi_1'), t_ind=0)

phi_1_pred.shape

(1, 21, 9)
第一个维度显示这种状态是标量，第二个维度显示horizon，第三个维度指的是所研究的九个不确定场景。

设置结果动画

将MPC结果动画化，以比较预测和闭环轨迹，可以对所获得的结果进行非常有意义的研究。
do-mpc显著促进了这一过程，同时与Matplotlib携手实现了完全的可定制性。获得可发布的动画就像编写以下短代码块一样简单：

from matplotlib.animation import FuncAnimation, FFMpegWriter, ImageMagickWriter

def update(t_ind):
    sim_graphics.plot_results(t_ind)
    mpc_graphics.plot_predictions(t_ind)
    mpc_graphics.reset_axes()

图形模块也可以不受限制地与加载的do mpc数据一起使用。这允许方便的数据后处理，例如在Jupyter笔记本中。我们只需要用上面加载的结果启动图形模块。

anim = FuncAnimation(fig, update, frames=20, repeat=False)
gif_writer = ImageMagickWriter(fps=3)
anim.save('anim.gif', writer=gif_writer)

下面我们展示了生成的gif文件（非实时）：

谢谢你，通过这个简短的例子如何使用做mpc。我们希望您能发现该工具和本文档非常有用。
我们建议您查看API文档，了解有关所提供模块、方法和函数的更多详细信息。
我们还想强调的是，我们在介绍中跳过了许多细节、进一步的功能等。请看一下我们更复杂的例子，以更好地了解do mpc的可能性。

运行代码(python)

FileNotFoundError: [Errno 2] No such file or directory: ‘convert’
解决方式
sudo apt-get install imagemagick

点击最下面的魔术命令（运行以上）就可以在Jupyter Notebook中看到以上输出的图片与数据

import numpy as np

# Add do_mpc to path. This is not necessary if it was installed via pip.
# import sys
# sys.path.append('../../')

# Import do_mpc package:
import do_mpc
'''
    do mpc的一个基本范式是模块化架构,根据应用程序的不同,单个积木可以独立使用,也可以联合使用。
    在下文中,我们将从模型开始介绍这些块之间的配置、设置和连接。
'''
model_type = 'continuous' # either 'discrete' or 'continuous'
model = do_mpc.model.Model(model_type)
phi_1 = model.set_variable(var_type='_x', var_name='phi_1', shape=(1,1))
phi_2 = model.set_variable(var_type='_x', var_name='phi_2', shape=(1,1))
phi_3 = model.set_variable(var_type='_x', var_name='phi_3', shape=(1,1))
# Variables can also be vectors: 变量也可以是向量
dphi = model.set_variable(var_type='_x', var_name='dphi', shape=(3,1))
# Two states for the desired (set) motor position: 所需（设定）电机位置的两种状态
phi_m_1_set = model.set_variable(var_type='_u', var_name='phi_m_1_set')
phi_m_2_set = model.set_variable(var_type='_u', var_name='phi_m_2_set')
# Two additional states for the true motor position: 真实电机位置的另外两种状态
phi_1_m = model.set_variable(var_type='_x', var_name='phi_1_m', shape=(1,1))
phi_2_m = model.set_variable(var_type='_x', var_name='phi_2_m', shape=(1,1))
#print('phi_1={}, with phi_1.shape={}'.format(phi_1, phi_1.shape))
#print('dphi={}, with dphi.shape={}'.format(dphi, dphi.shape))
# As shown in the table above, we can use Long names or short names for the variable type.
# 如上表所示,我们可以对变量类型使用长名称或短名称
Theta_1 = model.set_variable('parameter', 'Theta_1')
Theta_2 = model.set_variable('parameter', 'Theta_2')
Theta_3 = model.set_variable('parameter', 'Theta_3')

c = np.array([2.697,  2.66,  3.05, 2.86])*1e-3
d = np.array([6.78,  8.01,  8.82])*1e-5
model.set_rhs('phi_1', dphi[0])
model.set_rhs('phi_2', dphi[1])
model.set_rhs('phi_3', dphi[2])
from casadi import *
dphi_next = vertcat(
    -c[0]/Theta_1*(phi_1-phi_1_m)-c[1]/Theta_1*(phi_1-phi_2)-d[0]/Theta_1*dphi[0],
    -c[1]/Theta_2*(phi_2-phi_1)-c[2]/Theta_2*(phi_2-phi_3)-d[1]/Theta_2*dphi[1],
    -c[2]/Theta_3*(phi_3-phi_2)-c[3]/Theta_3*(phi_3-phi_2_m)-d[2]/Theta_3*dphi[2],
)

model.set_rhs('dphi', dphi_next)
tau = 1e-2
model.set_rhs('phi_1_m', 1/tau*(phi_m_1_set - phi_1_m))
model.set_rhs('phi_2_m', 1/tau*(phi_m_2_set - phi_2_m))
model.setup()

mpc = do_mpc.controller.MPC(model)

setup_mpc = {
    'n_horizon': 20,
    't_step': 0.1,
    'n_robust': 1,
    'store_full_solution': True,
}
mpc.set_param(**setup_mpc)

mterm = phi_1**2 + phi_2**2 + phi_3**2
lterm = phi_1**2 + phi_2**2 + phi_3**2

mpc.set_objective(mterm=mterm, lterm=lterm)
mpc.set_rterm(
    phi_m_1_set=1e-2,
    phi_m_2_set=1e-2
)
# Lower bounds on states:
mpc.bounds['lower','_x', 'phi_1'] = -2*np.pi
mpc.bounds['lower','_x', 'phi_2'] = -2*np.pi
mpc.bounds['lower','_x', 'phi_3'] = -2*np.pi
# Upper bounds on states
mpc.bounds['upper','_x', 'phi_1'] = 2*np.pi
mpc.bounds['upper','_x', 'phi_2'] = 2*np.pi
mpc.bounds['upper','_x', 'phi_3'] = 2*np.pi

# Lower bounds on inputs:
mpc.bounds['lower','_u', 'phi_m_1_set'] = -2*np.pi
mpc.bounds['lower','_u', 'phi_m_2_set'] = -2*np.pi
# Lower bounds on inputs:
mpc.bounds['upper','_u', 'phi_m_1_set'] = 2*np.pi
mpc.bounds['upper','_u', 'phi_m_2_set'] = 2*np.pi
mpc.scaling['_x', 'phi_1'] = 2
mpc.scaling['_x', 'phi_2'] = 2
mpc.scaling['_x', 'phi_3'] = 2
inertia_mass_1 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_2 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_3 = 2.25*1e-4*np.array([1.])

mpc.set_uncertainty_values(
    Theta_1 = inertia_mass_1,
    Theta_2 = inertia_mass_2,
    Theta_3 = inertia_mass_3
)

mpc.setup()
simulator = do_mpc.simulator.Simulator(model)
# Instead of supplying a dict with the splat operator (**), as with the optimizer.set_param(),
# we can also use keywords (and call the method multiple times, if necessary):
simulator.set_param(t_step = 0.1)
p_template = simulator.get_p_template()
# type(p_template)
# p_template.keys()
def p_fun(t_now):
    p_template['Theta_1'] = 2.25e-4
    p_template['Theta_2'] = 2.25e-4
    p_template['Theta_3'] = 2.25e-4
    return p_template
simulator.set_p_fun(p_fun)
simulator.setup()
x0 = np.pi*np.array([1, 1, -1.5, 1, -1, 1, 0, 0]).reshape(-1,1)
simulator.x0 = x0
mpc.x0 = x0

# mpc.x0['phi_1']
mpc.set_initial_guess()

# Customizing Matplotlib:
import matplotlib.pyplot as plt
import matplotlib as mpl
mpl.rcParams['font.size'] = 18
mpl.rcParams['lines.linewidth'] = 3
mpl.rcParams['axes.grid'] = True
mpc_graphics = do_mpc.graphics.Graphics(mpc.data)
sim_graphics = do_mpc.graphics.Graphics(simulator.data)
#%%capture
# We just want to create the plot and not show it right now. This "inline magic" supresses the output.
fig, ax = plt.subplots(2, sharex=True, figsize=(16,9))
fig.align_ylabels()
#%%capture
for g in [sim_graphics, mpc_graphics]:
    # Plot the angle positions (phi_1, phi_2, phi_2) on the first axis:
    g.add_line(var_type='_x', var_name='phi_1', axis=ax[0])
    g.add_line(var_type='_x', var_name='phi_2', axis=ax[0])
    g.add_line(var_type='_x', var_name='phi_3', axis=ax[0])

    # Plot the set motor positions (phi_m_1_set, phi_m_2_set) on the second axis:
    g.add_line(var_type='_u', var_name='phi_m_1_set', axis=ax[1])
    g.add_line(var_type='_u', var_name='phi_m_2_set', axis=ax[1])


ax[0].set_ylabel('angle position [rad]')
ax[1].set_ylabel('motor angle [rad]')
ax[1].set_xlabel('time [s]')
u0 = np.zeros((2,1))
for i in range(200):
    simulator.make_step(u0)
sim_graphics.plot_results()
# Reset the limits on all axes in graphic to show the data.
sim_graphics.reset_axes()
# Show the figure:
fig
#%%capture
u0 = mpc.make_step(x0)
sim_graphics.clear()
mpc_graphics.plot_predictions()
mpc_graphics.reset_axes()
# Show the figure:
fig
#%%capture

# mpc_graphics.pred_lines
# mpc_graphics.pred_lines['_x', 'phi_1']
# Change the color for the three states:
for line_i in mpc_graphics.pred_lines['_x', 'phi_1']: line_i.set_color('#1f77b4') # blue
for line_i in mpc_graphics.pred_lines['_x', 'phi_2']: line_i.set_color('#ff7f0e') # orange
for line_i in mpc_graphics.pred_lines['_x', 'phi_3']: line_i.set_color('#2ca02c') # green
# Change the color for the two inputs:
for line_i in mpc_graphics.pred_lines['_u', 'phi_m_1_set']: line_i.set_color('#1f77b4')
for line_i in mpc_graphics.pred_lines['_u', 'phi_m_2_set']: line_i.set_color('#ff7f0e')

# Make all predictions transparent:
for line_i in mpc_graphics.pred_lines.full: line_i.set_alpha(0.2)
# Get line objects (note sum of lists creates a concatenated list)
lines = sim_graphics.result_lines['_x', 'phi_1']+sim_graphics.result_lines['_x', 'phi_2']+sim_graphics.result_lines['_x', 'phi_3']

ax[0].legend(lines,'123',title='disc')

# also set legend for second subplot:
lines = sim_graphics.result_lines['_u', 'phi_m_1_set']+sim_graphics.result_lines['_u', 'phi_m_2_set']
ax[1].legend(lines,'12',title='motor')
simulator.reset_history()
simulator.x0 = x0
mpc.reset_history()
#%%capture
for i in range(20):
    u0 = mpc.make_step(x0)
    x0 = simulator.make_step(u0)
# Plot predictions from t=0
mpc_graphics.plot_predictions(t_ind=0)
# Plot results until current time
sim_graphics.plot_results()
sim_graphics.reset_axes()
fig
# 下方单元没看懂
# %%
from do_mpc.data import save_results, load_results
save_results([mpc, simulator])
# #with open(dompctest, 'rb') as f:
#     #results = pickle.load(f)
# results = load_results('/home/zhangjx/桌面/mult_BlueRov/MPC/results/results.pkl')
# %%
from matplotlib.animation import FuncAnimation, FFMpegWriter, ImageMagickWriter

def update(t_ind):
    sim_graphics.plot_results(t_ind)
    mpc_graphics.plot_predictions(t_ind)
    mpc_graphics.reset_axes()
anim = FuncAnimation(fig, update, frames=20, repeat=False)
gif_writer = ImageMagickWriter(fps=3)
anim.save('anim.gif', writer=gif_writer)

# %%