MPC理论推导

MPC理论推导

MPC是工程中常用到的控制器，其核心思想是以优化的方法求解最优控制器，其中优化方法大多时候采用二次规划QP(Quadratic Programming)求解。MPC与LQR的区别是LQR需要线性模型，MPC可以是线性或者非线性的模型。

问题描述

首先建立系统状态空间模型：
$$x(k+1)=Ax(k)+Bu(k)$$

MPC的思想是，在$t$时刻建立一个有限时域$N$内的代价函数$J$（LQR建立的是无线时域的代价函数），找某个控制律$u(t|t)$，使得代价函数最小，这里建立得代价函数为：
$$\begin{array}{rl}& J=\sum _{i=0}^{N-1}(x_i^{T}Q{x}_i+u_i^{T}R{u}_i)+x_N^T{Q}_f{x}_N\\ & \begin{array}{rl}s.t.& x_{i+1}=A{x}_i+B{u}_i\\ & u_{min}\le u_i\le u_{max}\\ & x_{min}\le x_i\le x_{max}\end{array}\end{array}\text{\hspace{1em}(1)}$$
式中，$x_i=x(t+i|t)$表示在$t$时刻时往前预测$i$步的系统状态；$u_i=u(t+i|t)$表示在$t$时刻时往前第i步的控制输入；$x_N=x(t+N|t)$表示终端时刻$N$的系统状态，这里单独给x_N设置一个代价项，是为了理论的完备性，这样能够从理论上证明MPC控制方法是收敛的；$Q$、$R$、$Q_f$分别表示系统状态、系统输入和终端状态的代价矩阵。

一般求解上面的代价函数用二次规划（QP）求解器，QP问题的标准形式为：
$$\begin{array}{rl}& J=\frac{1}{2}{x}^{T}Px+f^{T}x\\ & \begin{array}{rl}s.t.& Gx\le h\end{array}\end{array}\text{\hspace{1em}(2)}$$
我们要做的就是把MPC代价函数（式(1)）转换为QP问题代价函数（式(2)），然后调用QP求解器进行求解。（注意这里的$x$与上面系统状态的$x_i$不是同一个意思，这里的$x$只是待优化决策变量的一个通用表示符号）

MPC问题转换为QP问题

目标函数转换

将预测窗口$N$内的系统状态$x_i$和系统状态$u_i$组成大的向量的形式：
$$\begin{array}{rl}& X(t)=[x_0^T,x_1^T,...,x_N^T{]}_{(N+1\times n_x,1)}^T\\ & U(t)=[u_0^T,u_1^T,...,u_{N-1}^T{]}_{(N\times n_u,1)}^T\end{array}$$
其中，$n_x,n_u$分别表示系统状态和控制输入的维度。

MPC的代价函数(1)可以写为：
$$J=X(t{)}^T\bar{Q}X(t)+U(t{)}^T\bar{R}U(t)\text{\hspace{1em}(3)}$$
其中$\bar{Q}$和$\bar{R}$矩阵及其维度分别为：
$$\bar{Q}={\left[\begin{array}{ccccc}Q& 0& 0& \cdots & 0\\ 0& Q& 0& \cdots & 0\\ 0& 0& Q& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & Q_f\end{array}\right]}_{((N+1)\times n_x,(N+1)\times n_x)},\bar{R}={\left[\begin{array}{ccccc}R& 0& 0& \cdots & 0\\ 0& R& 0& \cdots & 0\\ 0& 0& R& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & R\end{array}\right]}_{(N\times n_u,N\times n_u)}$$
根据状态转移约束关系$x_{i+1}=A{x}_i+B{u}_i$可以得到$X(t)$和$U(t)$之间的关系为：
$$\begin{array}{rl}& x(t|t)=x(t|t)\\ & x(t+1|t)=Ax(t|t)+Bu(t|t)\\ & x(t+2|t)=Ax(t+1|t)+Bu(t+1|t)=A^{2}x(t|t)+ABu(t|t)+Bu(t+1|t)\\ & x(t+3|t)=Ax(t+2|t)+Bu(t+2|t)=A^{3}x(t|t)+A^{2}Bu(t|t)+ABu(t+1|t)+Bu(t+2|t)\\ ⋮& \\ & x(t+N|t)=Ax(t+N-1|t)+Bu(t_N-1|t)=A^{N}x(t|t)+A^{N-1}Bu(t+1|t)+...+Bu(t+N-1|t)\end{array}$$
写成矩阵形式为：
$$X(t)=\left[\begin{array}{c}x(t|t)\\ x(t+1|t)\\ x(t+2|t)\\ ⋮\\ x(t+N|t)\end{array}\right]=\left[\begin{array}{c}I\\ A\\ A^2\\ ⋮\\ A^N\end{array}\right]x(t|t)+\left[\begin{array}{ccccc}0& 0& 0& \cdots & 0\\ B& 0& 0& \cdots & 0\\ AB& B& Q& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ A^{N-1}B& A^{N-2}B& A^{N-3}& \cdots & B\end{array}\right]\left[\begin{array}{c}u(t|t)\\ u(t+1|t)\\ u(t+2|t)\\ ⋮\\ u(t+N-1|t)\end{array}\right]$$
令：
$$M=\left[\begin{array}{c}I\\ A\\ A^2\\ ⋮\\ A^N\end{array}\right],C=\left[\begin{array}{ccccc}0& 0& 0& \cdots & 0\\ B& 0& 0& \cdots & 0\\ AB& B& Q& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ A^{N-1}B& A^{N-2}B& A^{N-3}& \cdots & B\end{array}\right]$$
那么上面的式子可以写为：
$$X(t)=Mx(t|t)+CU(t)\text{\hspace{1em}(4)}$$
把式(4)代入到式(3)中可以得到MPC的代价函数为：
$$J=U^T(t)(C^T\bar{Q}C+\bar{R})U(t)+2x_0^T{M}^T\bar{Q}CU(t)+x_0^T{M}^T\bar{Q}M{x}_0$$
对比标准QP问题的代价函数形式：$J=\frac{1}{2}{x}^{T}Px+f^{T}x$，$x_0^T{M}^T\bar{Q}M{x}_0$为常量，可以不考虑到优化问题中，那么令：
$$P=2C^T\bar{Q}C+\bar{R},f=2C^T{\bar{Q}}^{T}M{x}_0$$

约束转换

MPC问题中的等式约束已经通过式(4)转换到了目标函数中，因此只需要考虑不等式约束转换为$Gx\le h$的形式，以$-1\le u_i\le 1$为例：
$$\begin{array}{rl}& U(t)=[u_0^T,u_1^T,...,u_{N-1}^T{]}_{(N\times n_u,1)}^T\\ & G=\left[\begin{array}{cc}{I}_{(N\times n_u)}& 0\\ 0& -I_{(N\times n_u)}\end{array}\right]\\ & h={\left[\begin{array}{c}1\\ 1\\ ⋮\\ 1\end{array}\right]}_{(2N\times n_u)}\end{array}$$

至此已经将MPC问题转换为QP问题，然后只需要调用QP求解器进行优化即可。

确定矩阵后，优化输入为当前$t$时刻的系统状态$x_0=x(t|t)$，优化输出为控制序列$U(t)$，由于理论构建的模型与系统真实模型存在偏差，优化所得的未来控制量对系统控制的价值很低，因此MPC仅执行输出序列$U(t)$中的第一个控制输出。

应用demo

MPC实现轨迹跟踪的demo可以参考这篇博客。