MPC轨迹跟踪控制器推导及Simulink验证

MPC轨迹跟踪控制器推导及Simulink验证

MPC的特点

1. 多变量控制：

一步都需要解决一个优化问题，特别是当系统变量和预测范围增大时，计算量会显著增加。
7. 需要精确模型：虽然MPC可以设计来应对模型不确定性，但其性能很大程度上依赖于模型的准确性。
8. 实现复杂：MPC的实现相对复杂，需要较高的专业知识和经验。
9. 与LQR相比：

• 计算较复杂：LQR控制器的求解相对简单，只需要解决一个线性二次调节器问题。
• 考虑约束：传统的LQR不考虑输入和状态约束。
• 灵活性：LQR通常优化一个固定的二次型目标，不如MPC灵活。

MPC轨迹跟踪控制器推导

一 系统离散化

对连续线性状态方程：
$$\{\begin{array}{rl}\dot{x}(t)& =Ax(t)+Bu(t)\\ y(t)& =Cx(t)+Du(t)\end{array}$$
其离散化状态方程为:
$$\{\begin{array}{rl}& x[k+1]=A_{d}x[k]+B_{d}u[k]\\ & y[k]=C_{d}x[k]+D_{d}u[k]\end{array}$$
其中，
$$\{\begin{array}{rl}{A}_d& =e^{AT}\\ B_d& =A^{-1}(e^{AT}-I)B\\ C_d& =C\\ D_d& =D\end{array}$$

二 预测区间状态和变量推导

$$X={\left[\begin{array}{c}x(k|k)\\ x(k+1|k)\\ x(k+2|k)\\ ⋮\\ x(k+N|k)\end{array}\right]}_{(N+1)n\times 1}$$

$$U={\left[\begin{array}{c}u(k|k)\\ u(k+1|k)\\ u(k+2|k)\\ ⋮\\ u(k+N-1|k)\end{array}\right]}_{Np\times 1}$$
根据离散化的状态空间方程
$$\{\begin{array}{rl}& x(k|k)=x_k\\ & x(k+1|k)=A_{d}x(k|k)+B_{d}u(k|k)=A_d{x}_k+B_{d}u(k|k)\\ & x(k+2|k)=A_{d}x(k+1|k)+B_{d}u(k+1|k)=A_d^2{x}_k+A_d{B}_{d}u(k|k)+u(k+1|k)\\ ⋮& \\ & x(k+N|k)=A_d^N{x}_k+A_d^{N-1}{B}_{d}u(k|k)+\cdots +u(k+N-1|k)\end{array}$$
化简得:
$$X_k=M_{(N+1)n\times n}{x}_k+C_{(N+1)n\times Np}{U}_k$$
其中:
$$\begin{gathered} M_{(N+1)n\times n}=\left[\begin{array}{c}I\\ A_d\\ A_d^2\\ ⋮\\ A_d^N\end{array}\right] \\ {C}_{(N+1)n\times Np}=\left[\begin{array}{cccc}O& & & \\ B_d& & & \\ A_d{B}_d& B_d& & \\ ⋮& ⋮& \ddots & \\ A_d^{N-1}{B}_d& A_d^{N-2}{B}_d& \dots & B_d\end{array}\right] \end{gathered}$$

三 代价函数推导

定义代价函数为状态误差加权和、控制输入加权和以及终端误差加权三者之和
$$J=\sum _k^{N-1}(E_k^{T}Q{E}_k+u_T^{k}R{u}_k)+E_N^{T}F{E}_N$$
其中，$E_k=y-r$,$y$为输出变量，r为参考向量，在倒立摆中，取$y=x$，故$E_k=x-ref$，化简代价函数为：
$$J={\left[\begin{array}{c}E(k|k)\\ E(k+1|k)\\ E(k+2|k)\\ ⋮\\ E(k+N|k)\end{array}\right]}^T\overline{Q}\left[\begin{array}{c}E(k|k)\\ E(k+1|k)\\ E(k+2|k)\\ ⋮\\ E(k+N|k)\end{array}\right]+{\left[\begin{array}{c}u(k|k)\\ u(k+1|k)\\ u(k+2|k)\\ ⋮\\ u(k+N-1|k)\end{array}\right]}^T\overline{R}\left[\begin{array}{c}u(k|k)\\ u(k+1|k)\\ u(k+2|k)\\ ⋮\\ u(k+N-1|k)\end{array}\right]$$
其中:

$$\{\begin{array}{rl}& E(k+i|k)=x(k+i|k)-ref,i=0,1,2,\dots ,N\\ & \overline{Q}=diag(Q,Q,Q,\dots ,F)\\ & \overline{R}=diag(R,R,R,\dots ,R)\end{array}$$
进一步展开得：
$$J=X_k^T\overline{Q}{X}_k-X^T\overline{Q}{R}_{ef}-R_{ef}^T\overline{Q}X+R_{ef}^T\overline{Q}{R}_{ef}+U_k^T\overline{R}{U}_k$$
易知：
$$\{\begin{array}{rl}& R_{ef}=[ref;ref;\dots ;ref]\\ & X^T\overline{Q}{R}_{ef}=R_{ef}^T\overline{Q}X\\ & X_k=M{x}_k+C{U}_k\end{array}$$
继续化简：
$$\begin{array}{rl}J& =(M{x}_k+C{U}_k{)}^T\overline{Q}(M{x}_k+C{U}_k)-2\ast (M{x}_k+C{U}_k{)}^T\overline{Q}{R}_{ef}+R_{ef}^T\overline{Q}{R}_{ef}+U_k^T\overline{R}{U}_k\\ & =x_k^T{M}^T\overline{Q}M{x}_k+2x_k^T{M}^T\overline{Q}C{U}_k+U_k^T{C}^T\overline{Q}C{U}_k-2x_k^T{M}^T\overline{Q}{R}_{ef}\\ & -2U_k^T{C}^T\overline{Q}{R}_{ef}+R_{ef}^T\overline{Q}{R}_{ef}+U_k^T\overline{R}{U}_k\\ & =x_k^T{M}^T\overline{Q}M{x}_k+2x_k^T{M}^T\overline{Q}C{U}_k-2x_k^T{M}^T\overline{Q}{R}_{ef}-2U_k^T{C}^T\overline{Q}{R}_{ef}+\\ & R_{ef}^T\overline{Q}{R}_{ef}+U_k^T(C^T\overline{Q}C+\overline{R})U_k\\ & =x_k^{T}G{x}_k+2x_k^{T}L{U}_k-2x_k^{T}P{R}_{ef}-2U_k^{T}S{R}_{ef}+R_{ef}^T\overline{Q}{R}_{ef}+U_k^{T}T{U}_k\\ & =x_k^{T}G{x}_k-2R_{ef}^T{P}^T{x}_k+R_{ef}^T\overline{Q}{R}_{ef}+2(x_k^{T}L-R_{ef}^T{S}^T)U_k+U_k^{T}T{U}_k\end{array}$$
其中：

$$\{\begin{array}{rl}& G=M^T\overline{Q}M\\ & L=M^T\overline{Q}C\\ & P=M^T\overline{Q}\\ & S=C^T\overline{Q}\\ & T=C^T\overline{Q}C+\overline{R}\end{array}$$

四 优化求解

通过3. 代价函数推导，采用二次优化方法使得代价$J$最小，进而进行控制，定义优化问题如下：
$$\begin{array}{rl}\underset{U_k}{\min }& J=U_k^{T}T{U}_k+2(x_k^{T}L-R_{ef}^T{S}^T)U_k\\ \text{subject to}& U_{min}\le U_k\le U_{max}\end{array}$$
在MATLAB中可使用函数quadprog(quadprog函数介绍)求解：

根据MATLAB中quadprog要求，定义:
$$\{\begin{array}{rl}H& =2T=2(C^T\overline{Q}C+\overline{R})\\ f& =2(x_k^{T}L-R_{ef}^T{S}^T{)}^T\\ A& =[]\\ b& =[]\\ A_{eq}& =[]\\ b_{eq}& =[]\\ lb& =U_{min}\\ ub& =U_{max}\end{array}$$
调用 quadprog(H,f,A,b,Aeq,beq,lb,ub) 即可求解代价函数$J$取最小值时，最优控制输入$u_k$。

基于MPC的倒立摆控制系统

采用这篇文章建立的倒立摆模型进行仿真测试。
在simulink中搭建控制仿真模型如下：

输入计算出的反馈增益$K$仿真结果如下：

相关资料

MATLAB代码和simulink模型点击这里

NMPC轨迹跟踪控制器推导及Simulink验证点击这里

欢迎大家与我交流！！！

Reference：

1. 推导倒立摆模型并使用LQR进行位置跟踪控制：https://blog.csdn.net/weixin_55249340/article/details/140421091?spm=1001.2014.3001.5501
2. MATLAB quadprog函数：https://ww2.mathworks.cn/help/optim/ug/quadprog_zh_CN.html?requestedDomain=cn