简单入门--带约束线性模型预测控制

---

本文将重点介绍MPC中的约束处理，这是MPC在实际应用中的重要组成部分，用于确保系统操作在安全、可行的范围内。

---

一、带约束线性模型预测控制

当系统模型为线性，即$x_{k+1}=A{x}_k+B{u}_k$, 且含状态约束和控制约束的模型预测控制为带约束线性模型预测控制。

1. 带约束线性模型预测控制问题-----标准型

标准式 P1:
$$\begin{array}{rlrlrl}& {\min }_{\bar{U}}& & \begin{array}{r}J\triangleq F(x_N)+\sum _{i=0}^{N-1}L(x_{k+i|k},u_{k+i|k})\end{array}& & 目标函数\\ & \text{s.t.:}& & x_{k+i+1|k}=A{x}_{k+i|k}+B{u}_{k+i|k}& & 系统模型\\ & & & A_c{x}_{k+i|k}\le b& & 状态约束\\ & & & u_{\min }\le u_{k+i|k}\le u_{\max }& & 控制量约束\\ & & & x_{k|k}=x_k& & 当前系统状态\\ & & & \bar{U}={\left[u_{k|k},\dots ,u_{k+i|k},\dots ,u_{k+N-1|k}\right]}^{\top }& & 待优化的控制策略\end{array}$$
这里目标函数通常选为$J\triangleq x_{k+N|k}^{\top }{Q}_N{x}_{k+N|k}+{\sum }_{i=0}^{N-1}\left(x_{k+i|k}^{\top }Q{x}_{k+i|k}+u_{k+i|k}^{\top }R{u}_{k+i|k}\right)$， 跟踪目标为系统原点；$x_{k+N|k}^{\top }{Q}_N{x}_{k+N|k}$终端误差指标，确保系统稳定性；$x_{k+i|k}^{\top }Q{x}_{k+i|k}$ 误差积分指标，越小表示误差积分越小；$u_{k+i|k}^{\top }R{u}_{k+i|k}$对控制量的指标，越小代表能量消耗越少；这里跟踪目标为系统原点。

2. 带约束线性模型预测控制问题-----QP问题

1、动态方程转换

a) 根据系统的状态方程$x_{k+i+1|k}=A{x}_{k+i|k}+B{u}_{k+i|k}$以及初值 $x_{k|k}=x_k$，递归推导得
$$\begin{array}{rl}& x_{k+1|k}=A{x}_{k|k}+B{u}_{k|k}\\ & x_{k+2|k}=A{x}_{k+1|k}+B{u}_{k+1|k}=A^2{x}_{k|k}+AB{u}_{k|k}+B{u}_{k+1}\\ & \dots \\ & x_{k+N|k}=A{x}_{k+N-1|k}+B{u}_{k+N_{-}1}=A^N{x}_k+A^{N-1}B{u}_k+\dots \dots +AB{u}_{k+N-2}+B{u}_{k+N-1}\end{array}$$

b) 上述方程可以重写为以下紧凑形式
$$\underset{\bar{X}}{\underbrace{\left[\begin{array}{c}{x}_{k|k}\\ x_{k+1|k}\\ x_{k+2|k}\\ \cdots \\ x_{k+N|k}\end{array}\right]}}=\underset{\bar{A}}{\underbrace{\left[\begin{array}{c}I\\ A\\ A^2\\ \cdots \\ A^N\end{array}\right]}}{x}_k+\underset{\bar{B}}{\underbrace{\left[\begin{array}{cccc}0& 0& ...& 0\\ B& 0& ...& 0\\ AB& B& ...& 0\\ \cdots & ...& ...& ...\\ A^{N-1B}& A^{N-2}B& ...& B\end{array}\right]}}\underset{\bar{U}}{\underbrace{\left[\begin{array}{c}{u}_k\\ u_{k+1}\\ \cdots \\ \cdots \\ u_{k+N-1}\end{array}\right]}}$$
即：
$$\begin{array}{r}\bar{X}=\bar{A}{x}_k+\bar{B}\bar{U}\end{array}$$
2. 目标函数转换
$$\begin{array}{rl}J=& {\bar{X}}^{\top }\underset{{\bar{Q}}_{(N+1)\times (N+1)}}{\underbrace{\left[\begin{array}{cccc}Q& & & \\ & \ddots & & \\ & & Q& \\ & & & Q_N\end{array}\right]}}\bar{X}+{\bar{U}}^{\top }\underset{{\bar{R}}_{N\times N}}{\underbrace{\left[\begin{array}{ccc}R& & \\ & \ddots & \\ & & R\end{array}\right]}}\bar{U}\\ =& {\bar{X}}^{\top }\bar{Q}\bar{X}+{\bar{U}}^{\top }\bar{R}\bar{U}\end{array}$$
将等式约束（1）带入目标函数
$$\begin{array}{rl}J=& {\bar{X}}^{\top }\bar{Q}\bar{X}+{\bar{U}}^{\top }\bar{R}\bar{U}\\ =& x_k^{\top }{\bar{A}}^{\top }\bar{Q}\bar{A}{x}_k+2x_k^{\top }{\bar{A}}^{\top }\bar{Q}\bar{B}\bar{U}+{\bar{U}}^{\top }{\bar{B}}^{\top }\bar{Q}\bar{B}\bar{U}+{\bar{U}}^{\top }\bar{R}\bar{U}\\ =& \frac{1}{2}{\bar{U}}^{\top }\underset{H}{\underbrace{2\left({\bar{B}}^{\top }\bar{Q}\bar{B}+\bar{R}\right)}}\bar{U}+\underset{f^{\top }}{\underbrace{2x_k^{\top }{\bar{A}}^{\top }\bar{Q}\bar{B}}}\bar{U}+\underset{c}{\underbrace{x_k^{\top }{\bar{A}}^{\top }\bar{Q}\bar{A}{x}_k}}\\ =& \frac{1}{2}{\bar{U}}^{\top }H\bar{U}+f^{\top }\bar{U}+c\end{array}$$
其中
$$H=2\left({\bar{B}}^{\top }\bar{Q}\bar{B}+\bar{R}\right),f=2{\bar{B}}^{\top }\bar{Q}\bar{A}{x}_k=G{x}_k$$
3. 约束转换
a) 状态约束
$$\begin{array}{r}\underset{{\bar{A}}_c}{\underbrace{\left[\begin{array}{ccccc}{A}_c& & & & \\ & A_c& & & \\ & & A_c& & \\ & & & \ddots & \\ & & & & A_c\end{array}\right]}}\underset{\bar{X}}{\underbrace{\left[\begin{array}{c}{x}_{k|k}\\ x_{k+1|k}\\ x_{k+2|k}\\ \cdots \\ x_{k+N|k}\end{array}\right]}}\le \underset{\bar{b}}{\underbrace{\left[\begin{array}{c}b\\ b\\ b\\ \cdots \\ b\end{array}\right]}}\end{array}$$
将等式约束（1）带入状态约束
$$\begin{array}{rl}& {\bar{A}}_c\bar{X}\le \bar{b}\\ & {\bar{A}}_c\bar{A}{x}_k+{\bar{A}}_c\bar{B}\bar{U}\le \bar{b}\\ & \underset{M}{\underbrace{{\bar{A}}_c\bar{B}}}\bar{U}\le \underset{N}{\underbrace{\bar{b}-{\bar{A}}_c\bar{A}{x}_k}}\end{array}$$
b) 控制约束
$$\begin{array}{r}\underset{{\bar{u}}_{\min }}{\underbrace{\left[\begin{array}{c}{u}_{\min }\\ u_{\min }\\ u_{\min }\\ \cdots \\ u_{\min }\end{array}\right]}}\le \underset{\bar{U}}{\underbrace{\left[\begin{array}{c}{u}_{k|k}\\ u_{k+1|k}\\ u_{k+2|k}\\ \cdots \\ u_{k+N-1|k}\end{array}\right]}}\le \underset{{\bar{u}}_{\max }}{\underbrace{\left[\begin{array}{c}{u}_{\max }\\ u_{\max }\\ u_{\max }\\ \cdots \\ u_{\max }\end{array}\right]}}\end{array}$$
4. 紧凑型
根据标准式 P1，（2）,(3) 和（4），可以得到无约束线性模型预测控制问题----QP问题：
$$\begin{array}{rlrlrl}& {\min }_{\bar{U}}& & \begin{array}{r}J=\frac{1}{2}{\bar{U}}^{\top }H\bar{U}+f^{\top }\bar{U}+c\end{array}& & 目标函数\\ & \text{s.t.:}& & \bar{M}\bar{U}\le \bar{N}& & 状态约束\\ & & & {\bar{u}}_{\min }\le \bar{U}\le {\bar{u}}_{\max }& & 状态约束\\ & & & \bar{U}={\left[u_{k|k},\dots ,u_{k+i|k},\dots ,u_{k+N-1|k}\right]}^{\top }& & 待优化的控制策略\end{array}$$

3. 带约束线性模型预测控制问题----- MATLAB求解QP问题

代码如下（示例）：

clear; clc; close all;
% 线性系统系数矩阵
A=[1 0 1 0; 
   0 1 0 1;
   0 0 1 0; 
   0 0 0 1]; 
B=[0 0;
   0 0;
   1 0;
   0 1];
% 初始状态量-如果不能在下一步回到约束范围内，则会造成无解
x0=[8; -4; -0.2;-0.1];
% 预测步长
N=5;
% 优化目标参数，加权矩阵
Q=[10 0  0 0;
    0 20 0 0; 
    0 0  100 0 ;
    0 0  0 100 ];
R=eye(size(B,2));

% 紧凑形式矩阵
At=zeros(size(A,1)*N,size(A,1));
Bt = zeros(size(A,1)*N,size(B,2)*N);    
for i=1:1:N
    At(size(A,1)*(i-1)+1:size(A,1)*i,:) = A^i;
    for j=1:1:i
        Bt(size(A,1)*(i-1)+1:size(A,1)*i,size(B,2)*(j-1)+1:size(B,2)*j)=A^(i-j)*B;
    end
end

Q_bar = kron(eye(N),Q);
R_bar = kron(eye(N),R);

H=2*(Bt'*Q_bar*Bt + R_bar);
G = Bt'*Q_bar*At;


%初始值
x=x0;
xk=x0;

for k=1:100
    %最优解
    U = -H\G*xk;
    % 采用优化得到的控制量的第一个元素作为实际作用的控制量，代入到原系统中得到下一个时刻的状态量
    u(:,k) = U(1:size(B,2));
    %动态方程更新
    x(:, k+1) = A*x(:, k) + B*u(:,k);
    xk = x(:, k+1);
end

%状态图
figure('Color','w');
plot(x(1,:),'c-','LineWidth',1.5);hold on
plot(x(2,:),'g-','LineWidth',1.5);hold on
plot(x(3,:),'b-','LineWidth',1.5);hold on
plot(x(4,:),'m-','LineWidth',1.5);hold on
legend('$x_1$','$x_2$','$x_3$','$x_4$','Interpreter','latex');
set(gca,'FontSize',20,'FontName','Times New Roman');

%控制量图
figure('Color','w');
plot(u(1,:),'c-','LineWidth',1.5);hold on
plot(u(2,:),'g-','LineWidth',1.5);hold on
legend('$u_1$','$u_2$','Interpreter','latex');
set(gca,'FontSize',20,'FontName','Times New Roman');