1 首先来说说LQR

1.1 Problem Statement

首先我们简单的假设一个物理系统，在光滑的一维地面上有一个质量为 $m$ 的滑块，初始位置与初始速度都为 $0$，现需要设计控制器，在传感器测得滑块位置 $x$ 的基础上，为滑块提供外力 $u$，使其跟随参考点 $x_r$ 。

建立动力学模型：

$$\ddot{x}=\frac{u}{m}$$

选取状态向量 $x=\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]=\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]$ ，这里我们将状态变量的x1等于位移x，状态变量x2等于速度，得到系统状态方程为：
$$\dot{x}=Ax+Bu$$
其中：
$$A=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right],B=\left[\begin{array}{c}0\\ \frac{1}{m}\end{array}\right]$$
我们需要的输出是 $x_1$（也就是我们的位移量），我们希望能通过控制 $u$来达到控制我们物块位移 $x_1$。

我们看到系统的开环矩阵 $A$ 就可以判断如果没有控制器这个系统是不可能稳定的，从物理角度也很好理解你给这个物块一个不变的 $u$，物块在光滑的地面上是不可能自己停下来的。

1.2 控制器

我们上节说到需要引入控制器才能使我们的系统稳定，我们考虑：
$$u=-kx=-\left[\begin{array}{c}{k}_1,k_2,\cdots \end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ ⋮\end{array}\right]$$
这样我们就可以得到一个新的闭环矩阵：
$$\begin{array}{rl}& \dot{x}=Ax-Bkx=(A-Bk)x\\ & \dot{x}=A_{cl}x\end{array}$$
也就是说我们通过选择k来改变 $A_{cl}$ 的特征值从而达到来控制系统的表现的目的。

所以问题就是我们如何来选择这个最好的 $k$ ？

我们有很多方法可以让系统达到稳定的状态，但是如何选取这个特征值将决定着我们让这个系统以一个怎么样的方式去靠近这个稳定状态。

所以带着对这句话的理解，我们来看LQR控制器的思想。

1.3 LQR(Linear Quadratic Regulator)

我们引入 Cost Function 的概念
$$J={\int }_0^{\infty }(x^{T}Qx+u^{T}Ru)dt$$
在这里Q矩阵 $\left[\begin{array}{ccc}a& 0& 0\\ 0& b& 0\\ 0& 0& c\end{array}\right]$ 的a,b,c对应的是 $x_1^2,x_2^2,x_3^2$ ，从这里看到我们希望x1对系统的影响大一点的话，我们就可以增加a的值，同理如果我们的R值很大，自然输入u对于系统的影响就会很大。

综上就是LQR可以在满足系统稳定性的同时，帮我们寻找到代价函数的最小值。
$$minJ={\int }_0^{\infty }(x^{T}Qx+u^{T}Ru)dt$$
现在我们回到刚才的小物块物理系统，我们现在希望小物块按照我们预设的轨迹 $x_r$运动，所以我们会有一个误差 $e$，那最终我们希望这个误差可以趋向于0，我们稍微更改一下我们现在的状态方程，我们用误差e来替换我们的x1，也就是
$$\dot{e}={\dot{x}}_r-{\dot{x}}_1=-{\dot{x}}_1=-x_2$$
那么得到新的状态方程
$$\left[\begin{array}{c}\dot{e}\\ {\dot{x}}_2\end{array}\right]=\left[\begin{array}{cc}0& -1\\ 0& 0\end{array}\right]\left[\begin{array}{c}e\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{m}\end{array}\right]u$$
假设在这里我们希望u对代价函数的影响更大一点，我们就分别设置Q和R为：
$$\begin{gathered} Q=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right] \\ R=10 \end{gathered}$$
利用Riccati方程计算

1.4 仿真建模

简单粗略地仿真建模，见谅 ＞ˍ＜

我们将初始值设在5，目标值设在1，最后得到

至此一个简单物理系统的LQR控制系统就OK了，接下来我们看一下更复杂一点的MPC。

2.接着说MPC

2.1 模型的离散化

还是之前的控制对象：
$$\dot{x}=Ax+Bu$$
我们用前向欧拉法将状态方程离散化：
$$\begin{array}{rl}& \dot{x}=\frac{x(k+1)-x(k)}{T}=Ax(k)+Bu(k)\\ & x(k+1)=(I+TA)x(k)+TBu(k)\\ & x(k+1)=\bar{A}x(k)+\bar{B}u(k)\end{array}$$
其中
$$\bar{A}=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right],\bar{B}=\left[\begin{array}{c}0\\ \frac{T}{m}\end{array}\right]$$
这里的T是控制周期。

2.2 Prediction预测

MPC方法的一个独特之处就是需要对未来系统状态进行预测，在k时刻我们预估未来p个控制周期内预测的系统状态为：
$$X_k={\left[\begin{array}{cccc}x(k+1|k{)}^T& x(k+2|k{)}^T& \cdots & x(k+p|k{)}^T\end{array}\right]}^T$$
p称为预测时域，括号中k+1|k表示在当前k时刻预测k+1时刻的系统状态，以此类推。

另外，预测动态系统未来状态时，还需要知道预测时域内的控制量Uk：
$$U_k={\left[\begin{array}{cccc}u(k|k{)}^T& u(k+1|k{)}^T& \cdots & u(k+p-1|k{)}^T\end{array}\right]}^T$$
这是我们接下来将要求解的优化问题的独立变量。

现在，我们可以通过离散化状态方程依次对未来p个控制周期的系统状态进行预测：
$$\begin{array}{rl}& x(k+1|k)=\bar{A}x(k)+\bar{B}u(k|k)\\ & x(k+2|k)={\bar{A}}^{2}x(k)+\bar{A}\bar{B}u(k|k)+\bar{B}u(k+1|k)\\ & x(k+3|k)={\bar{A}}^{3}x(k)+{\bar{A}}^2\bar{B}u(k|k)+\bar{A}\bar{B}u(k+1|k)+\bar{B}u(k+2|k)\\ & ⋮\\ & x(k+p|k)={\bar{A}}^{p}x(k)+\sum _{i=0}^{p-1}{\bar{A}}^{p-1-i}\bar{B}u(k+i|k)\end{array}$$

整合成矩阵形式：
$$X_k=\Psi x(k)+\Theta U_k$$
其中
$$\Psi =\left[\begin{array}{c}{\bar{A}}^1\\ {\bar{A}}^2\\ ⋮\\ {\bar{A}}^p\end{array}\right],\Theta =\left[\begin{array}{cccc}{\bar{A}}^{1-1}\bar{B}& \cdots & 0& 0\\ {\bar{A}}^{2-1}\bar{B}& {\bar{A}}^{2-2}\bar{B}& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\bar{A}}^{p-1}\bar{B}& {\bar{A}}^{p-2}\bar{B}& \cdots & {\bar{A}}^{p-p}\bar{B}\end{array}\right]$$

上式中的下三角形式，直接反映了系统在时间上的因果关系，即k+1时刻的输入对k时刻的输出没有影响，k+2时刻的输入对k和k+1时刻没有影响。

2.3 优化

这一节我们将求解预测时域内的控制输出$U_k$，在求解优化问题之前，我们首先明确优化问题的数学描述。

我们的控制目标是使系统的状态跟踪期望的一条轨迹，通常称为参考值，定义预测时域内的参考值序列：
$$R_k={\left[\begin{array}{cccc}r(k+1{)}^T& r(k+2{)}^T& \cdots & x(k+p{)}^T\end{array}\right]}^T$$
注意，在k时刻进行控制的时候，控制器就必须已经得到了k时刻到k+p时刻的参考值，而PID就不需要这么多信息，这是MPC的一个缺点。

我们希望寻找最佳的控制量$U_k$，使得预测时域内的状态向量与参考值越接近越好，这是一个开环最优控制问题。为此，我们用预测状态向量与参考值之间的累计误差定义一个简单的优化目标函数：
$$J(U_k)=(X_k-R_k{)}^{T}Q(X_k-R_k)$$

经常地，我们不希望控制动作太大，优化目标函数再添加一项对控制量的约束：
$$J(U_k)=(X_k-R_k{)}^{T}Q(X_k-R_k)+U_k^{T}W{U}_k$$

因此，该优化问题可以描述如下：
$$minJ(U_k)$$

我们将优化函数J(Uk)展开后合并同类项：
$$\begin{array}{rl}J(U_k)& =(X_k-R_k{)}^{T}Q(X_k-R_k)+U_k^{T}W{U}_k\\ & =(E+\Theta U_k{)}^{T}Q(E+\Theta U)+U_k^{T}W{U}_k\\ & =E^{T}QE+(\Theta U_k{)}^{T}Q(\Theta U_k)+2E^{T}Q(\Theta U_k)+U_k^{T}W{U}_k\\ & =U_k^T{\Theta }^{T}Q\Theta U_k+U_k^{T}W{U}_k+2E^{T}Q(\Theta U_k)+E^{T}QE\\ & =U_k^T({\Theta }^{T}Q\Theta +W)U_k+2(E^{T}Q\Theta )U_k+E^{T}QE\end{array}$$

上式中 是常数项，对“$U_k$为何值时$J$取得最小值”这一问题没有影响，因此直接舍去。

如图，matlab输入 “help quadprog”查看二次型优化函数quadprog的说明文档，令
$$\{\begin{array}{rl}& H=2({\Theta }^{T}Q\Theta +W)\\ & f^T=2E^{T}Q\Theta \end{array}$$

可得最终优化目标函数，至此可直接调用matlab quadprog函数求解$U_k$，将$U_k$的第一个元素提取出来，作为本控制周期的控制量。
$$J(U_k)=\frac{1}{2}{U}_k^{T}H{U}_k+f^{T}x$$

2.4 仿真

我们对动力学方程两边拉普拉斯变换：
$$s^{2}X(s)=\frac{1}{m}F(s)$$
得到传递函数为：
$$G(s)=\frac{X(s)}{F(s)}=\frac{1}{m{s}^2}$$
建立仿真：

我们得到在固定值和sinwave的情况下基本都可以跟踪的比较好（参数还可继续优化）

其中MPC代码为：

function u = Controller(pos_ref, pos, vel)
%参数设置
m    = 1.05;                %滑块质量,增加了5%作为建模误差
T    = 0.01;                %控制周期10ms
p    = 40;                  %控制时域（预测时域）
Q    = 10*eye(2*p);         %累计误差权重
W    = 0.0001*eye(p);       %控制输出权重
umax = 100;                 %控制量限制，即最大的力
Rk   = zeros(2*p,1);        %参考值序列
Rk(1:2:end) = pos_ref;    
Rk(2:2:end) = vel;          %参考速度跟随实际速度
%构建中间变量
xk    = [pos;vel];          %xk
A_    = [1 T;0 1];          %离散化预测模型参数A
B_    = [0;T/m];            %离散化预测模型参数B
psi   = zeros(2*p,2);       %psi
for i=1:1:p
    psi(i*2-1:i*2,1:2)=A_^i;
end
theta = zeros(2*p,p);       %theta
for i=1:1:p
    for j=1:1:i
        theta(i*2-1:i*2,j)=A_^(i-j)*B_;
    end
end
E = psi*xk-Rk;              %E
H = 2*(theta'*Q*theta+W);   %H
f = (2*E'*Q*theta)';        %f
%优化求解
coder.extrinsic('quadprog');
Uk=quadprog(H,f,[],[],[],[],-umax,umax);
%返回控制量序列第一个值
u = 0.0;                    %显示指定u的类型
u = Uk(1);