编程语言
首页 > 编程语言> > 【附C++源代码】模型预测控制(MPC)公式推导以及算法实现,Model Predictive control介绍

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

作者:互联网

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

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

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

主要内容如下:

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

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

x k + 1 = A x k + B u k (1) \pmb{x_{k+1}} = \pmb A \pmb{x_k} + \pmb B \pmb{u_k} \tag{1} xk+1​​xk+1​​​xk+1​=AAAxk​​xk​​​xk​+BBBuk​​uk​​​uk​(1)

其中, x \pmb x xxx是系统的状态变量; u \pmb u uuu是对系统控制的输入量; A , B \pmb A,B AAA,B均为定值矩阵,用来对状态和控制量进行转换。

上式(1)是状态的一步传递。

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

为了方便大家的理解,我把上述过程通过公式的方式写出来:
x x , x = I ∗ x x , x + 0 ∗ u k , k x x + 1 , x = A ∗ x x , x + B ∗ u k , k x x + 2 , x = A ∗ x x + 1 , x + B ∗ u k + 1 , k = A ∗ ( A ∗ x x , x + B ∗ u k , k ) + B ∗ u k + 1 , k ⋮ x x + N , x = A N x k , k + A N − 1 B u k , k + A N − 1 B u k + 1 , k + ⋯ (3) \pmb x_{x,x} = \pmb I* \pmb x_{x,x} + \pmb 0 * \pmb u_{k,k} \\ \pmb x_{x+1,x} = \pmb A* \pmb x_{x,x} + \pmb B * \pmb u_{k,k} \\ \pmb x_{x+2,x} = \pmb A* \pmb x_{x+1,x} + \pmb B * \pmb u_{k+1,k} = \pmb A* (\pmb A* \pmb x_{x,x} + \pmb B * \pmb u_{k,k}) + \pmb B * \pmb u_{k+1,k} \\ \vdots \\ \pmb x_{x+N,x} = \pmb A^{N} \pmb x_{k,k} + \pmb A^{N-1} \pmb B \pmb u_{k,k} + \pmb A^{N-1} \pmb B u_{k+1, k} + \cdots \tag{3} xxxx,x​=III∗xxxx,x​+000∗uuuk,k​xxxx+1,x​=AAA∗xxxx,x​+BBB∗uuuk,k​xxxx+2,x​=AAA∗xxxx+1,x​+BBB∗uuuk+1,k​=AAA∗(AAA∗xxxx,x​+BBB∗uuuk,k​)+BBB∗uuuk+1,k​⋮xxxx+N,x​=AAANxxxk,k​+AAAN−1BBBuuuk,k​+AAAN−1BBBuk+1,k​+⋯(3)

上述过程中的控制量 u k \pmb u_k uuuk​很关键,只有知道了它的值,我们才能使用上面的过程进行状态的预测。而这个 u k \pmb u_k uuuk​便是我们通过MPC求解得到的。

我们将上述的(3)式写成一个向量和矩阵的形式,则如下:

[ x k , k x k + 1 , k x k + 2 , k ⋮ x k + N , k ] = [ I A A 2 ⋮ A N ] x k + [ 0 0 ⋯ 0 B 0 ⋯ 0 A B B ⋯ 0 A N − 1 N A N − 2 ⋯ B ] [ u k , k u k + 1 , k u k + 2 , k ⋮ u k + N − 1 , k ] (4) \left[ \begin{matrix} \pmb x_{k,k} \\ \pmb x_{k+1,k} \\ \pmb x_{k+2,k} \\ \vdots \\ \pmb x_{k+N,k} \end{matrix} \right] = \left[ \begin{matrix} \pmb I \\ \pmb A \\ \pmb A^2 \\ \vdots \\ \pmb A^N \end{matrix} \right] \pmb x_k + \left[ \begin{matrix} \pmb 0 & \pmb 0 & \cdots & \pmb 0 \\ \pmb B & \pmb 0 & \cdots & \pmb 0 \\ \pmb A \pmb B & \pmb B & \cdots & \pmb 0 \\ \pmb A^{N-1} \pmb N & \pmb A^{N-2} & \cdots & \pmb B \\ \end{matrix} \right] \left[ \begin{matrix} \pmb u_{k,k} \\ \pmb u_{k+1,k} \\ \pmb u_{k+2,k} \\ \vdots \\ \pmb u_{k+N-1,k} \end{matrix} \right] \tag{4} ⎣⎢⎢⎢⎢⎢⎡​xxxk,k​xxxk+1,k​xxxk+2,k​⋮xxxk+N,k​​⎦⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎡​IIIAAAAAA2⋮AAAN​⎦⎥⎥⎥⎥⎥⎤​xxxk​+⎣⎢⎢⎡​000BBBAAABBBAAAN−1NNN​000000BBBAAAN−2​⋯⋯⋯⋯​000000000BBB​⎦⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎡​uuuk,k​uuuk+1,k​uuuk+2,k​⋮uuuk+N−1,k​​⎦⎥⎥⎥⎥⎥⎤​(4)

将上式进一步简写得到:
X k = M x k + C U k (5) \pmb X_k = \pmb M \pmb x_k + \pmb C \pmb U_k \tag{5} XXXk​=MMMxxxk​+CCCUUUk​(5)

在此,再次强调,式子(5)是通过 k k k时刻的已知状态 x k \pmb x_k xxxk​,通过 N N N个控制量 u k + i , k \pmb u_{k+i,k} uuuk+i,k​对未来的 N N N个状态进行预测而得到的,务必要理解这一点。

(2).构建误差函数

想象一下,我们有了对未来的 N N N个预测状态, x k , k \pmb x_{k,k} xxxk,k​、 x k + 1 , k \pmb x_{k+1,k} xxxk+1,k​、 x k + 2 , k \pmb x_{k+2,k} xxxk+2,k​、 x k + 3 , k \pmb x_{k+3,k} xxxk+3,k​ …

而在多数的控制任务中,我们同样也有一串连续的已知参考状态, r k \pmb r_{k} rrrk​、 r k + 1 \pmb r_{k+1} rrrk+1​、 r k + 2 \pmb r_{k+2} rrrk+2​、 r k + 3 \pmb r_{k+3} rrrk+3​ …

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

这样说可能有一些抽象,我们以一个一维的直线跟踪为例子,通过图示进行解释:
请添加图片描述
上图 x x x轴为要跟踪的参考直线,我们以窗口N=6进行状态量预测,得到在 k k k时刻的6个预测量,如上图中的红点所示,各个预测量对应的参考量为x轴的蓝点。

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

假设我们的参考状态始终为 r = [ 0 , 0 ] \pmb r = [0,0] rrr=[0,0],那么每一个预测状态的误差就为 e = x k , k − r k = x k , k e = \pmb x_{k,k} - \pmb r_{k} = \pmb x_{k,k} e=xxxk,k​−rrrk​=xxxk,k​。类似的可以写出整个窗口之内的误差为:

J = x k + N , k T F x k + N , k + ∑ i = 0 N − 1 x k + i , k T Q x k + i , k (6) \pmb J = \pmb x_{k+N, k}^T \pmb F \pmb x_{k+N, k} + \sum_{i=0}^{N-1} \pmb x_{k+i, k}^T \pmb Q \pmb x_{k+i, k} \tag{6} JJJ=xxxk+N,kT​FFFxxxk+N,k​+i=0∑N−1​xxxk+i,kT​Q​Q​​Qxxxk+i,k​(6)

上式(6)中的 F \pmb F FFF和 Q \pmb Q Q​Q​​Q分别表示误差的权重,它们都为对角矩阵,其中的数值越大,在优化的时候就越倾向于将当前这个式子的值减少的越多。

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

J = x k + N , k T F x k + N , k + ∑ i = 0 N − 1 ( x k + i , k T Q x k + i , k + u k + i , k T R u k + i , k ) (7) \pmb J = \pmb x_{k+N, k}^T \pmb F \pmb x_{k+N, k} + \sum_{i=0}^{N-1} (\pmb x_{k+i, k}^T \pmb Q \pmb x_{k+i, k} + \pmb u_{k+i,k}^T \pmb R \pmb u_{k+i,k})\tag{7} JJJ=xxxk+N,kT​FFFxxxk+N,k​+i=0∑N−1​(xxxk+i,kT​Q​Q​​Qxxxk+i,k​+uuuk+i,kT​RRRuuuk+i,k​)(7)

因此就得到了完整的误差函数 J \pmb J JJJ,我们的目的是通过优化(7)式,得到使整个误差函数最小的控制量 u \pmb u uuu.

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

为了计算方便,将(7)式子写成矩阵的形式:

J = x k T G x k + U k T H U k + 2 U k T E x k (8) J = \pmb x_k^T \pmb G\pmb x_k + \pmb U_k^T \pmb H \pmb U_k + 2\pmb U_k^T \pmb E \pmb x_k \tag{8} J=xxxkT​GGGxxxk​+UUUkT​HHHUUUk​+2UUUkT​EEExxxk​(8)

其中: G = M T Q ‾ M \pmb G = \pmb M^T \overline{\pmb{Q}}\pmb M GGG=MMMTQ​Q​​Q​MMM, E = C T Q ‾ M \pmb E = \pmb C^T \overline{\pmb{Q}}\pmb M EEE=CCCTQ​Q​​Q​MMM, H = C T Q ‾ C R ‾ \pmb H = \pmb C^T \overline{\pmb{Q}}\pmb C \overline{\pmb{R}} HHH=CCCTQ​Q​​Q​CCCRRR

Q ‾ = [ Q 0 ⋯ 0 0 0 Q ⋯ 0 0 Q ⋯ ⋮ ⋮ ⋮ ⋱ 0 0 0 ⋯ F ] \overline{\pmb{Q}} = \left[ \begin{matrix} \pmb Q & \pmb 0 & \cdots & \pmb 0 & \pmb 0 \\ \pmb 0 & \pmb Q & \cdots \\ \pmb 0 & \pmb 0 & \pmb Q & \cdots \\ \vdots & \vdots & \vdots & \ddots \\ \pmb 0 & \pmb 0 & \pmb 0 & \cdots & \pmb F \\ \end{matrix} \right] Q​Q​​Q​=⎣⎢⎢⎢⎢⎢⎡​Q​Q​​Q000000⋮000​000Q​Q​​Q000⋮000​⋯⋯Q​Q​​Q⋮000​000⋯⋱⋯​000FFF​⎦⎥⎥⎥⎥⎥⎤​

R ‾ = [ R 0 ⋯ 0 0 0 R ⋯ 0 0 R ⋯ ⋮ ⋮ ⋮ ⋱ 0 0 0 ⋯ R ] \overline{\pmb{R}} = \left[ \begin{matrix} \pmb R & \pmb 0 & \cdots & \pmb 0 & \pmb 0 \\ \pmb 0 & \pmb R & \cdots \\ \pmb 0 & \pmb 0 & \pmb R & \cdots \\ \vdots & \vdots & \vdots & \ddots \\ \pmb 0 & \pmb 0 & \pmb 0 & \cdots & \pmb R \\ \end{matrix} \right] RRR=⎣⎢⎢⎢⎢⎢⎡​RRR000000⋮000​000RRR000⋮000​⋯⋯RRR⋮000​000⋯⋱⋯​000RRR​⎦⎥⎥⎥⎥⎥⎤​

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

式子(7)是一个无约束的最优化问题,也可以直接求导,导数为零的点即为最优控制量。也就是(7)式的最优解为:
U ∗ = H − 1 ( − E ∗ x k ) (9) \pmb U^* = \pmb H^{-1} (-\pmb E * \pmb x_k) \tag{9} UUU∗=HHH−1(−EEE∗xxxk​)(9)

(4).如何使用控制量 U ∗ U^* U∗

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

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

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

首先对公式(1)进行实例化,得到如下具体系统:

x k + 1 = A x k + B u k \pmb{x_{k+1}} = \pmb A \pmb{x_k} + \pmb B \pmb{u_k} xk+1​​xk+1​​​xk+1​=AAAxk​​xk​​​xk​+BBBuk​​uk​​​uk​

[ x 1 , k + 1 x 2 , k + 1 ] = [ 1 0.1 0 2 ] [ x 1 , k x 2 , k ] + [ 0 0.5 ] ∗ u k (10) \left[\begin{matrix} x_{1,k+1} \\ x_{2,k+1} \end{matrix}\right] =\left[\begin{matrix} 1 & 0. 1 \\ 0 & 2 \end{matrix}\right] \left[\begin{matrix} x_{1,k} \\ x_{2,k} \end{matrix}\right]+ \left[\begin{matrix} 0 \\ 0.5 \end{matrix}\right] * \pmb u_k \tag{10} [x1,k+1​x2,k+1​​]=[10​0.12​][x1,k​x2,k​​]+[00.5​]∗uuuk​(10)

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

对应的 Q \pmb Q Q​Q​​Q、 R \pmb R RRR、 F \pmb F FFF矩阵分别如下:

Q = [ 1 0 0 1 ] \pmb Q = \left[\begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix}\right] Q​Q​​Q=[10​01​]

F = [ 2 0 0 2 ] \pmb F = \left[\begin{matrix} 2 & 0 \\ 0 & 2 \end{matrix}\right] FFF=[20​02​]

R = [ 0.1 ] \pmb R = \left[\begin{matrix} 0.1 \end{matrix}\right] RRR=[0.1​]

初始状态为:
x i n i t = [ 5 5 ] \pmb x_{init} = \left[\begin{matrix} 5 \\ 5 \end{matrix}\right] xxxinit​=[55​]

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

如果大家对于矩阵的应用不是很熟悉,可能在计算每个矩阵维度的时候,会有一些困难。为了大家的理解,我把(10)所示的系统,在预测窗口 N = 3 N=3 N=3时的各个矩阵的维度都列出来,方便代价在推导公式的时候,进行验证。
Q \pmb Q Q​Q​​Q:2x2
F \pmb F FFF:2x2
R \pmb R RRR:1x1
x k \pmb x_k xxxk​:2x1
A \pmb A AAA:2x2
B \pmb B BBB:2x1
M \pmb M MMM:8x2
C \pmb C CCC:8x3
Q ‾ \overline{\pmb Q} Q​Q​​Q​:8x8
R ‾ \overline{\pmb R} RRR:3x3
G \pmb G GGG:2x2
E \pmb E EEE:3x2
H \pmb H HHH: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 \pmb Q Q​Q​​Q、 R \pmb R RRR、 F \pmb F FFF,以及预测区间 N N N对控制器的影响。

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

(1). 预测区间N的影响
请添加图片描述

(2). 权重矩阵F的影响
请添加图片描述
(3). 权重矩阵Q的影响
请添加图片描述
(4). 权重矩阵R的影响
请添加图片描述

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

4. 总结

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

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

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

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

5.参考资料

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

标签:control,Predictive,matrix,uuuk,state,num,pmb,xxxk,源代码
来源: https://blog.csdn.net/u011341856/article/details/122799600