【附C++源代码】模型预测控制(MPC)公式推导以及算法实现，Model Predictive control介绍

2022年的第一篇博客，首先祝大家新年快乐！

提示：本篇博客主要集中在对MPC的理解以及应用。这篇博客可以作为你对MPC控制器深入研究的一个开始，起到抛砖引玉，带你快速了解其原理的作用。

这篇博客将介绍一下模型预测控制器(MPC)的公式、推导以及C++代码的实现。

主要内容如下：

• 从一个简单的线性系统开始，对MPC控制器公式进行推导；
• 根据推导出来的结论，对一个具体的系统进行控制，使用C++对MPC进行实现；
• 通过实验找到参数对该系统的影响。

1. 模型预测控制(MPC)公式推导

(1). 系统方程建模
假设，我们有以下的一个线性离散系统：

$$x_{k+1}\text{xk+1}{x}_{k+1}=A\text{A}A{x}_k\text{xk}{x}_k+B\text{B}B{u}_k\text{uk}{u}_k\text{\hspace{1em}(1)}$$

其中，$x\text{x}x$是系统的状态变量；$u\text{u}u$是对系统控制的输入量；$A\text{A}A,B$均为定值矩阵，用来对状态和控制量进行转换。

上式（1）是状态的一步传递。

假设我们以状态${x\text{x}x}_{k,k}$为基础，然后以控制量${u\text{u}u}_{k,k}$，带入式子（1）将计算得到${x\text{x}x}_{k+1,k}$，然后再将计算出来的${x\text{x}x}_{k+1,k}$和${u\text{u}u}_{k+1,k}$带入（1）式，又得到了${x\text{x}x}_{k+2,k}$，以此类推，就可以得到在$k$时刻的时候，对未来$N$个状态的预测。

为了方便大家的理解，我把上述过程通过公式的方式写出来：
$$\begin{gathered} {x\text{x}x}_{x,x}=I\text{I}I\ast {x\text{x}x}_{x,x}+0\text{0}0\ast {u\text{u}u}_{k,k} \\ {x\text{x}x}_{x+1,x}=A\text{A}A\ast {x\text{x}x}_{x,x}+B\text{B}B\ast {u\text{u}u}_{k,k} \\ {x\text{x}x}_{x+2,x}=A\text{A}A\ast {x\text{x}x}_{x+1,x}+B\text{B}B\ast {u\text{u}u}_{k+1,k}=A\text{A}A\ast (A\text{A}A\ast {x\text{x}x}_{x,x}+B\text{B}B\ast {u\text{u}u}_{k,k})+B\text{B}B\ast {u\text{u}u}_{k+1,k} \\ ⋮ \\ {x\text{x}x}_{x+N,x}={A\text{A}A}^N{x\text{x}x}_{k,k}+{A\text{A}A}^{N-1}B\text{B}B{u\text{u}u}_{k,k}+{A\text{A}A}^{N-1}B\text{B}B{u}_{k+1,k}+\cdots \text{\hspace{1em}(3)} \end{gathered}$$

上述过程中的控制量${u\text{u}u}_k$很关键，只有知道了它的值，我们才能使用上面的过程进行状态的预测。而这个${u\text{u}u}_k$便是我们通过MPC求解得到的。

我们将上述的（3）式写成一个向量和矩阵的形式，则如下：

$$\left[\begin{array}{c}{x\text{x}x}_{k,k}\\ {x\text{x}x}_{k+1,k}\\ {x\text{x}x}_{k+2,k}\\ ⋮\\ {x\text{x}x}_{k+N,k}\end{array}\right]=\left[\begin{array}{c}I\text{I}I\\ A\text{A}A\\ {A\text{A}A}^2\\ ⋮\\ {A\text{A}A}^N\end{array}\right]{x\text{x}x}_k+\left[\begin{array}{cccc}0\text{0}0& 0\text{0}0& \cdots & 0\text{0}0\\ B\text{B}B& 0\text{0}0& \cdots & 0\text{0}0\\ A\text{A}AB\text{B}B& B\text{B}B& \cdots & 0\text{0}0\\ {A\text{A}A}^{N-1}N\text{N}N& {A\text{A}A}^{N-2}& \cdots & B\text{B}B\end{array}\right]\left[\begin{array}{c}{u\text{u}u}_{k,k}\\ {u\text{u}u}_{k+1,k}\\ {u\text{u}u}_{k+2,k}\\ ⋮\\ {u\text{u}u}_{k+N-1,k}\end{array}\right]\text{\hspace{1em}(4)}$$

将上式进一步简写得到：
$${X\text{X}X}_k=M\text{M}M{x\text{x}x}_k+C\text{C}C{U\text{U}U}_k\text{\hspace{1em}(5)}$$

在此，再次强调，式子（5）是通过$k$时刻的已知状态${x\text{x}x}_k$，通过$N$个控制量${u\text{u}u}_{k+i,k}$对未来的$N$个状态进行预测而得到的，务必要理解这一点。

(2).构建误差函数

想象一下，我们有了对未来的$N$个预测状态，${x\text{x}x}_{k,k}$、${x\text{x}x}_{k+1,k}$、${x\text{x}x}_{k+2,k}$、${x\text{x}x}_{k+3,k}$ …

而在多数的控制任务中，我们同样也有一串连续的已知参考状态，${r\text{r}r}_k$、${r\text{r}r}_{k+1}$、${r\text{r}r}_{k+2}$、${r\text{r}r}_{k+3}$ …

对于控制任务，控制器要做的就是通过对系统输入控制量，从而使得状态接近参考状态。

这样说可能有一些抽象，我们以一个一维的直线跟踪为例子，通过图示进行解释：

上图$x$轴为要跟踪的参考直线，我们以窗口N=6进行状态量预测，得到在$k$时刻的6个预测量，如上图中的红点所示，各个预测量对应的参考量为x轴的蓝点。

很明显，当红点与对应的蓝点之间的距离越接近的时候，控制的效果肯定就是越好。按照这个思路，我们可以将控制过程，写成一个代价函数，然后通过优化的方式进行求解。

假设我们的参考状态始终为$r\text{r}r=[0,0]$，那么每一个预测状态的误差就为$e={x\text{x}x}_{k,k}-{r\text{r}r}_k={x\text{x}x}_{k,k}$。类似的可以写出整个窗口之内的误差为：

$$J\text{J}J={x\text{x}x}_{k+N,k}^{T}F\text{F}F{x\text{x}x}_{k+N,k}+\sum _{i=0}^{N-1}{x\text{x}x}_{k+i,k}^{T}Q\text{Q}Q{x\text{x}x}_{k+i,k}\text{\hspace{1em}(6)}$$

上式（6）中的$F\text{F}F$和$Q\text{Q}Q$分别表示误差的权重，它们都为对角矩阵，其中的数值越大，在优化的时候就越倾向于将当前这个式子的值减少的越多。

式子（6）中只是对状态的误差进行优化，这似乎有一些不合理，因为这完全没有考虑到控制量，如果将控制量也加入到（6）式中，可以得到：

$$J\text{J}J={x\text{x}x}_{k+N,k}^{T}F\text{F}F{x\text{x}x}_{k+N,k}+\sum _{i=0}^{N-1}({x\text{x}x}_{k+i,k}^{T}Q\text{Q}Q{x\text{x}x}_{k+i,k}+{u\text{u}u}_{k+i,k}^{T}R\text{R}R{u\text{u}u}_{k+i,k})\text{\hspace{1em}(7)}$$

因此就得到了完整的误差函数$J\text{J}J$，我们的目的是通过优化（7）式，得到使整个误差函数最小的控制量$u\text{u}u$.

以上过程牵扯到一些最优化的思想，如果你觉得难以理解，可以看一些优化相关的理论。

为了计算方便，将（7）式子写成矩阵的形式：

$$J={x\text{x}x}_k^{T}G\text{G}G{x\text{x}x}_k+{U\text{U}U}_k^{T}H\text{H}H{U\text{U}U}_k+2{U\text{U}U}_k^{T}E\text{E}E{x\text{x}x}_k\text{\hspace{1em}(8)}$$

其中：$G\text{G}G={M\text{M}M}^T\overline{Q\text{Q}Q}M\text{M}M$，$E\text{E}E={C\text{C}C}^T\overline{Q\text{Q}Q}M\text{M}M$，$H\text{H}H={C\text{C}C}^T\overline{Q\text{Q}Q}C\text{C}C\overline{R\text{R}R}$

$$\overline{Q\text{Q}Q}=\left[\begin{array}{ccccc}Q\text{Q}Q& 0\text{0}0& \cdots & 0\text{0}0& 0\text{0}0\\ 0\text{0}0& Q\text{Q}Q& \cdots & & \\ 0\text{0}0& 0\text{0}0& Q\text{Q}Q& \cdots & \\ ⋮& ⋮& ⋮& \ddots & \\ 0\text{0}0& 0\text{0}0& 0\text{0}0& \cdots & F\text{F}F\end{array}\right]$$

$$\overline{R\text{R}R}=\left[\begin{array}{ccccc}R\text{R}R& 0\text{0}0& \cdots & 0\text{0}0& 0\text{0}0\\ 0\text{0}0& R\text{R}R& \cdots & & \\ 0\text{0}0& 0\text{0}0& R\text{R}R& \cdots & \\ ⋮& ⋮& ⋮& \ddots & \\ 0\text{0}0& 0\text{0}0& 0\text{0}0& \cdots & R\text{R}R\end{array}\right]$$

(3).代价函数求解
对于式子（7）它是一个典型的二次规划问题，它是一个高维度的凸函数，它有且只有一个极值点，并且还是它的最小值。对于最小值的求解可以使用现成的优化库，例如OSQP等等进行求解。

式子（7）是一个无约束的最优化问题，也可以直接求导，导数为零的点即为最优控制量。也就是（7）式的最优解为：
$${U\text{U}U}^{\ast }={H\text{H}H}^{-1}(-E\text{E}E\ast {x\text{x}x}_k)\text{\hspace{1em}(9)}$$

(4).如何使用控制量$U^{\ast }$

通过（9）式子，得到了$N$个预测的控制量，但是实际使用的时候，只取第一个控制量，作为$k$时刻的控制量。然后计算得到$K+1$时刻的状态，再以$k+1$时刻的状态，重复上述过程进行预测控制的计算。

2.具体例子以及代码实现

为了更方便的理解上面的过程，我引入一个实际的例子。

首先对公式（1）进行实例化，得到如下具体系统：

$$x_{k+1}\text{xk+1}{x}_{k+1}=A\text{A}A{x}_k\text{xk}{x}_k+B\text{B}B{u}_k\text{uk}{u}_k$$

$$\left[\begin{array}{c}{x}_{1,k+1}\\ x_{2,k+1}\end{array}\right]=\left[\begin{array}{cc}1& 0.1\\ 0& 2\end{array}\right]\left[\begin{array}{c}{x}_{1,k}\\ x_{2,k}\end{array}\right]+\left[\begin{array}{c}0\\ 0.5\end{array}\right]\ast {u\text{u}u}_k\text{\hspace{1em}(10)}$$

并且，假设系统的预测区间$N=3$

对应的$Q\text{Q}Q$、$R\text{R}R$、$F\text{F}F$矩阵分别如下：

$$Q\text{Q}Q=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$

$$F\text{F}F=\left[\begin{array}{cc}2& 0\\ 0& 2\end{array}\right]$$

$$R\text{R}R=\left[\begin{array}{c}0.1\end{array}\right]$$

初始状态为：
$${x\text{x}x}_{init}=\left[\begin{array}{c}5\\ 5\end{array}\right]$$

将上面的几个参数值代入到第1节中的公式（4）（5）（8）中，构建所需要的大矩阵，然后使用（9）式即可完成最优控制量的求解。

* 有一点再次强调，计算出来的控制量一共N个，但是我们只取第一个作为当前时刻的控制量，然后传入控制系统中。

如果大家对于矩阵的应用不是很熟悉，可能在计算每个矩阵维度的时候，会有一些困难。为了大家的理解，我把（10）所示的系统，在预测窗口$N=3$时的各个矩阵的维度都列出来，方便代价在推导公式的时候，进行验证。
$Q\text{Q}Q$：2x2
$F\text{F}F$：2x2
$R\text{R}R$：1x1
${x\text{x}x}_k$：2x1
$A\text{A}A$：2x2
$B\text{B}B$：2x1
$M\text{M}M$：8x2
$C\text{C}C$：8x3
$\overline{Q\text{Q}Q}$：8x8
$\overline{R\text{R}R}$：3x3
$G\text{G}G$：2x2
$E\text{E}E$：3x2
$H\text{H}H$：3x3

MPC的代码片段如下：(代码中的字母和这篇博客的字母是一一对应的)

Eigen::VectorXd RunMPC(unsigned int N, Eigen::VectorXd &init_x, Eigen::MatrixXd &A, Eigen::MatrixXd &B,
                       Eigen::MatrixXd &Q, Eigen::MatrixXd &R, Eigen::MatrixXd &F) {

    unsigned int num_state = init_x.rows();
    unsigned int num_control = B.cols();

    Eigen::MatrixXd C, M;
    C.resize((N + 1) * num_state, num_control * N);
    C.setZero();
    M.resize((N + 1) * num_state, num_state);
    Eigen::MatrixXd temp;
    temp.resize(num_state, num_state);
    temp.setIdentity();
    M.block(0, 0, num_state, num_state).setIdentity();
    for (unsigned int i = 1; i <= N; ++i) {
        Eigen::MatrixXd temp_c;
        temp_c.resize(num_state, (N + 1) * num_control);
        temp_c << temp * B, C.block(num_state * (i - 1), 0, num_state, C.cols());

        C.block(num_state * i, 0, num_state, C.cols())
                = temp_c.block(0, 0, num_state, temp_c.cols() - num_control);

        temp = A * temp;
        M.block(num_state * i, 0, num_state, num_state) = temp;
    }

    Eigen::MatrixXd Q_bar, R_bar;

    Q_bar.resize(num_state * (N + 1), num_state * (N + 1));
    Q_bar.setZero();
    for (unsigned int i = 0; i < N; ++i) {
        Q_bar.block(num_state * i, num_state * i, num_state, num_state) = Q;
    }
    Q_bar.block(num_state * N, num_state * N, num_state, num_state) = F;

    R_bar.resize(N * num_control, N * num_control);
    R_bar.setZero();
    for (unsigned int i = 0; i < N; ++i) {
        R_bar.block(i * num_control, i * num_control, num_control, num_control) = R;
    }


    Eigen::MatrixXd G = M.transpose() * Q_bar * M;
    Eigen::MatrixXd E = C.transpose() * Q_bar * M;
    Eigen::MatrixXd H = C.transpose() * Q_bar * C + R_bar;

    return H.inverse() * (-E * init_x);
}

>>完整代码，点击进入<<

3.实验结果

对于公式（10）所示的系统，我们来探讨$Q\text{Q}Q$、$R\text{R}R$、$F\text{F}F$，以及预测区间$N$对控制器的影响。

公式（10）所示的系统，其状态量有2个，其中$x_2$收敛的比较快，不利于我们观察，所以下图只是绘制了第一个状态量变量的变化情况。

(1). 预测区间N的影响

(2). 权重矩阵F的影响

(3). 权重矩阵Q的影响

(4). 权重矩阵R的影响

上面的过程只是为了帮助大家去理解MPC各个权重参数的大致作用，所得出的结论并一定适合别的系统。

4. 总结

本文从一个离散的线性系统开始，对MPC进行推导，完整的探索了整个过程。

如果大家对优化理论和控制理论有一些了解的话，对于MPC的理解是比较容易的，它并不复杂。

我们在上述的过程中，构建了一个代价函数$J\text{J}J$，它的最优值求解是一个二次规划问题，在不考虑约束的情况下，直接求导就能得到最优的控制量。

但是实际应用中，可能还会引入一些约束。例如，在自动驾驶情境中，我们对四轮汽车的系统，控制它的速度和转角，但是这些控制量很明显有实际意义，汽车的转角有一个范围，速度也有范围，因此我们在求解类似这样的控制问题时，需要对代价函数加入约束。此时就变成了带约束的二次规划问题了，就不能再通过求导得到最优值了，只能通过优化迭代的方式找到最优值。

5.参考资料

(1). MPC模型预测控制器