ModulePredictiveControl.md · 飞行器智能感知与控制/learn_control

# Module Predictive Control

[TOC]

## 1. 数学基础

在之前的seminar中，我介绍了传统控制理论的内容。传统控制理论以Laplace变换为基础，通过对系统进行Laplace变换，从而使用传递函数来描述一个系统输入与输出之间的关系。这无疑是行之有效的，在这个基础上发展出了大量且可靠的工具。

但是现实生活中很少有简单的单输入单输出的问题，一个系统常常受到多个输入和输出共同作用，此时如果继续采用传递函数来描述系统会变得十分复杂（需要对系统解耦后进行控制）。此外传递函数将系统看作一个黑箱，只关心输入与输出之间的关系，并不关心输入对系统内部造成了怎样的影响。系统内部具体的结构是怎么样的我们并不清楚。

### 1.1 系统的状态空间描述

### 1.1.1 状态

在使用状态空间法描述一个系统的动态特征之前，需要先确定用于描述系统的状态向量组。状态变量组是能够**完全确定动力学系统**运动状态（描述系统时域行为）的**最少个数**的一组**独立变量**。核心在于使用状态变量组能够完整的描述系统的**动态特征**，这里的动态特征可能比较难以理解。

动态特征指的是系统中会随着时间变化而变化的特征，一般来讲指的是需要通过微分方程来描述的特征；想象一个如下通过代数方程来描述的系统：
$$
x = A+Bu
$$
可以通过解该方程来获得状态$x$的形式，但是此时获得的是一个简单的常数，状态的值是不发生变化的，并不是“**动态特征**”。那么如何分辨系统的动态特征呢？

事实上系统的动态特征可以理解为对**能量的存储**。如果直接使用代数方程描述，那么系统的状态变化只于当前的输入有关系，只与系统的输入有关，就像上一个例子所示。当系统存在动态特征时，系统的状态变化不只与当前的输入有关了，还与系统之前的状态有关，这可以理解为系统对能量的一种存储。

想象这样一个电学例子：

该系统由一个电容、电阻和电感串联而成，系统的输入为$u(t)$，系统的输出为电容上的电压。对于该系统而言，储能元件主要为电容和电感，电阻并不具有储能作用。但是电容和电感能独立的储存能量，因此电容上的电压和电感上的电压应是两个独立的状态变量。

状态变量描述了系统内部的特征，一般表示为：
$$
x = [x_1, x_2, \dots, x_n ]^T
$$
状态变量组描述了系统的内部特征。如果使用输入输出描述的话，对于上面这个电学系统，输出只为干路上的电流，系统内部电容和电感的特征被隐藏起来了。在使用状态方程描述时，我们可以很清晰的看到系统内部电容和电感上电压的变化过程，甚至可以对其进行控制；这也是状态方程描述相对于传递函数的办法的一个优势。

### 1.1.2 状态方程

对于上面这个系统，我们可以通过微分方程来描述：
$$
C \frac{du_c}{dt} = i\\
L\frac{i}{dt} +Ri+u_c= u
$$
化简可得：
$$
\dot u_c = \frac{1}{C}i\\
\dot i = -\frac 1 Lu_c - \frac{R}{L}i+\frac{1}{L}u
$$
可以将上式使用矩阵形式描述，如下所示：
$$
\left [
	\begin{matrix}
		\dot u_c \\
		\dot i
	\end{matrix}
\right]
=
\left [
	\begin{matrix}
		0 & \frac 1 C \\
        -\frac 1 L & -\frac R L \\
	\end{matrix}
\right]
\left [
	\begin{matrix}
		u_c \\
		i
	\end{matrix}
\right]
+
\left [
	\begin{matrix}
		0\\
		\frac 1 L
	\end{matrix}
\right]

u
$$
其中，状态变量组为：
$$
x = \left [
	\begin{matrix}
		u_c \\
		i
	\end{matrix}
\right]
$$
我们可以归纳性的总结出线性状态方程的一般形式。假设对于一个系统的状态变量组为$x$，输出为$y$，则其线性状态方程组如下所示：
$$
\dot x  = Ax + Bu\\
y = Cx + Du
$$
这个方程就是系统的状态方程组，对于该方程组有如下解释：

1. 其中第一个方程是一个一阶微分方程，描述了系统的动态特征。即通过该方程可以实现对未来时间的预测，是我们主要关心的对象。
2. 第二个方程描述了系统从状态到输出的线性组合，系统的输出可以随意选取，但是其不包含系统的动态特征，输出只由系统的状态和输出直接决定，我们一般并不关心。

### 1.2 连续系统离散化

连续系统离散化的方法很多，但是一般我们使用欧拉法进行离散化。

一般称欧拉法为向前法，可以简单的理解为向前差分，采用如下公式来近似微分：
$$
\dot x=  \frac {x(k+1)- x(k)}{T}
$$
其中$T$是采样周期，$\dot x$是上一时刻的微分。

将之代入线性系统方程中，可得：
$$
x(k+1) = (TA + I)x(k) + TBu(k)\\
y(k) = Cx(k) + Du(k)
$$
令：$\Phi = I+TA, G=TB, H=C, J=D$，则离散化后的状态空间方程为：
$$
x(k+1) = \Phi x(k) + Gu(k)\\
y(k) = Hx(k) + Ju(k)
$$

### 1.3 状态反馈控制器

在传统的控制办法中，通过系统的输出来建立反馈控制器。但是对于通过状态空间描述的系统而言，可以通过系统状态变量的线性组合来构成系统的反馈控制输入。系统的状态反馈控制器可以写成如下形式：
$$
u = -Kx
$$
其中$K$是静态常数矩阵，将上式代入状态空间模型，可得**闭环系统的状态空间模型**：
$$
\dot x = (A-KB)x\\
y = (C-BD)x
$$
要分析系统进行控制后的动态特征，只需要分析微分方程$\dot x = (A-KB)x$的动态特征就可以了。

## 2. LQR（Linear Quadratic Regulator）

## 2.1 引言

对于多输入多输出问题（MIMO），有这样一个简单的例子：

对于如上所是的一个MIMO系统，系统的输入$u_1、u_2$与系统的输出$y_1、y_2$之间会相互影响。如果该系统希望使用PID控制算法进行控制，需要设计两个相互独立的PID控制器。但是这也意味着这两个独立的PID控制器之间没有任何交联，使得系统难以设计，如果像该图一样设计一个较大的系统，则参数较多难以实现，这时就需要使用以LQR为代表的现代控制办法了。

## 2.2 理论推导

### 2.2.1 线性二次型最优控制 LQ Optimal Control

我们有一个线性系统如下所是：
$$
\dot{x} = A(t)x+B(t)u\\
y= C(t)x+D(t)u
$$
那么定义一个连续时域的泛函（函数的函数）代价函数如下所示：
$$
J=\frac{1}{2}\int^{t_f}_{t_0}[x(t)^TQx(t)+u(t)^TRu(t)]dt+\frac{1}{2}x^T(t_f)Q_1x(t_f)
$$
$t_0，t_f$是控制的起始和终止时间，需要求解一个最有的控制时间序列$u^*(t)$使得系统稳定且使上式取极值，这样的问题就是一个LQ最优控制问题。

在讨论如何让这个代价函数取最优之前，先讨论一下其中各个形式的含义：

1. **$L_1 = x(t)^TQx(t)$：**其中$x(t)$是状态向量；$Q$是权重矩阵。我们要求Q阵是半正定的，既有$x(t)^TQx(t)>0$，即总有$L_1$非负；往往Q矩阵是对角矩阵，对角线上的元素取大于或等于0的常值或变量时，有$L_1=\frac{1}{2}qx^2$。$L_1$非负，且$L_1$的形式很接近物理学中“**能量**“的概念。所以可以简单的认为$L_1$描述了系统从开始运作到达到目标所消耗的总能量。因此$L_1$衡量了误差$x(t)$与$0$状态之间误差的大小。当$J$最小时，$L_1$也会尽可能的小，满足最小误差向量$x(t)$能尽可能的小。

2. $L_2 = u(t)^TRu(t)：$其中$u(t)$是控制的输入，$R$是权重矩阵。因为我们的最终目标是获得一个最有的$u(t)$输入序列，因此$u(t)$一定是非0的，则$L_2$也一定大于0 。和$L_1$类似，$L_2$衡量控制所消耗的能量就会永远的大于0，就是控制所付出的代价。

LQ optimal control 是一个**finite horizon optimal problem(有限时域优化问题)**，因为积分的上限是一个有限值，控制任务希望控制器能够在**有限时间内**把状态转到到零点或者某一个平衡点，保证泛函取极值。在这里就是希望能够在不消耗过多能量的情况下把状态误差保持到最小。这样的控制器也叫 **Finite Horizon LQR.**

有限时域LQR的求解过程我们这里不追究了，最后解得结果：
$$
K(t)=-R(t)^{-1}(B^T(t)P(t)+N^T)\\
u^*(t) = K(t)x(t)
$$
其中$P(t)$是Riccati matrix differential equation的解。
$$
A^TP(t) + P(t)A - (P(t)B+N)R^{-1}(B^TP(t)_N^T) + Q = -\dot P(t)
$$
此时我们发现(3)构成了一个**线性状态反馈**，只不过**反馈增益矩阵是时变的**。Finite Horizon LQR的求解比较繁琐，主要是求解并不容易。

### 2.2.2 无限时域LQR Infinite Horizon LQR

由于上式中的积分上限是有限的，最终$P(t)$总是一个微分方程，导致增益矩阵是时变的。我们后来发现，当$t_f \to \infty$时，理查德方程由微分方程退化为一个代数方程，此时反馈矩阵变为一个恒定的值。此时最优控制的代价函数如下：
$$
J = \frac 1 2 \int ^\infty_{t_0}[x(t)^TQx(t) + u(t)^TRu(t)]dt
$$
原本的理查德方程的微分方程退化为如下的代数方程(Algebraic Riccati Equation(ARE))

$$
PA + A^TP-PBR^{-1}B^TP = -Q
$$
通过解该方程可得反馈矩阵：
$$
K = -R^{-1}B^TP
$$
此时系统的状态反馈如下所示：
$$
u^*(t) = Kx(t)
$$
此时控制**仍然是一个线性状态反馈**，而且**反馈增益矩阵还是一个常值矩阵**。

对于LQR控制器，有如下特点：

1. LQR显然是**同时适用于SISO和MIMO系统**的一种控制器。它的实质与状态反馈没有什么差别，只不过通过了优化性能函数得到一组此意义下的最优增益矩阵。所以说上一篇文章中的MIMO反馈增益即可以通过Pole-Placement，也可以通过LQR配置。
2. Infinite Horizon LQR**只适用于线性系统**，并且**要求系统是能控的**。因为如果系统不能控，$L_2$项在无限时域中最后是不会收敛的，所有$u(t)$都会使得整个积分(2)都会成为无穷大，没有优劣可比。Finite Horizon LQR是没有关系的，因为$L_2$肯定是有界的。
3. (4)和(7)构成了一个闭环LTI系统，且闭环系统**特征值实部均为负，即渐进稳定**。
4. LQR所采用的是静态增益矩阵$K$，所以闭环系统的阶数并不会变化。

## 3.MPC（Model Predictive Control）

### 3.1 简介

#### 3.1.1 为什么要用MPC

LQR方法已经可以很好的解决MIMO系统的控制问题了，为什么要使用MPC呢？事实上LQR是一种离线的控制策略。控制器的所有参数在机器人工作之前就已经设计好了，无论机器人发生了怎样的变化参数都不会发生任何改变。这导致机器人的鲁棒性不那么强。而MPC事实上是一种在线控制的策略，具体的作用机理将在下一小节中叙述， 它会根据实际的机器人行为和模型预测来更性控制器参数。这赋予了控制算法更强的适应能力。很有趣的一点是，现有的四足机器人多使用MPC控制算法进行控制，这是因为四足机器人在触地和离地两个状态中负载差异极大，很难用一个简单的模型来描述，因此需要使用状态预测对控制器。

#### 3.1.2 作用机理

模型预测控制的设计目标是计算未来控制变量 $u$ 的轨迹，用以优化未来系统的输出 $y$。优化过程在一个有限的时间窗口进行，并且利用优化时间窗口开始时的系统信息进行优化。

可以用更加准确的预言来表述MPC的工作过程：在**每一个采用时刻**，根据获得的当前测量信息，在线求解一个有限时间开环优化问题，并将得到的控制序列的**第一个元素作用于被控对象**。在下一个采样时刻，重复上述过程：用新的测量值作为此时预测系统未来动态的初始条件，刷新优化问题并重新求解 。

那么MPC算法包括三个步骤：

1. 以当前的观测为基础预测系统未来的动态
2. 求解优化问题
3. 将优化解的第一个元素（或者说第一个部分）作用于系统

这三步是在每个采样时刻重复进行的，且无论采用什么样的模型，每个采样时刻得到的测量值都作为当前时刻预测系统未来动态的初始条件。

在线求解开环优化问题获得开环优化序列是MPC和传统控制方法的主要区别，因为后者通常是离线求解一个反馈控制律，并将得到的反馈控制律一直作用于系统。

**需要注意的是：**

1. *MPC是一个反馈控制策略，但是之前不是说将得到的控制序列中的第一个元素作用于被控对象，求解开环问题。那么哪来的反馈呢？*

这是因为实际上预测需要当前的观测值来参与，每个采样时刻得到的测量值都作为当前时刻预测系统未来动态的初始条件 。

2. *传统的控制方法为什么被称为离线控制？*

设计变阻器应有的级数来达到控制并励直流电动机启动的例子，那么如何看的出来是离线控制呢？其实很简单，在起始的时候就已经把每级电阻值就给定出来了，但是实际上是否能如理论计算的这样呢?比如我电压突然受到了扰动了怎么办？那么这个计算的每级电阻值肯定不对了啦，但是传统的控制方法对此确是无能为力的，因为每级启动电阻在最早的时候已经计算好了的。

### 3.2 理论推导

#### 3.2.1 模型预测

我们先从一个简单的形式出发，观察一个线性定长系统的状态方程：
$$
\dot x = A x + Bu\\
y = Cx+Du
$$
在进行控制时，我们往往只惯性状态方程中的第一个方程。该方程描述了状态的变化，只要能将状态控制住，作为状态的线性组合的输出自然也被控制住了。因此在之后的分析中我们不去惯性系统输出$y$的变化。对第一个方程使用欧拉法将状态方程离散化，则有：
$$
x(k+1) = (I+TA)x(k)+Bu(t)
$$
其令：$\Phi = I+TA, G=TB$，则离散化后的状态空间方程为：
$$
x(k+1) = \Phi x(k) + Gu(k)
$$
我们需要对未来$p$个控制周期内的状态进行预测，并将预测的状态记作$X_k$，则有：
$$
X_k =
\left [
	\begin{matrix}
	
		x(k|k)^T & x(k+1|k)^T& x(k+2|k)^T& \dots  x(k+p|k)^T
	\end{matrix}
\right]^T
$$
其中$p$成为预测时域，括号中$k+1|k$表示在$k$时刻对$k+1$时刻的状态的预测。此外系统预测的目标是达到预测的状态所需要的控制量序列$U_k$：
$$
U_k =
\left [
	\begin{matrix}
		u(k|k)^T& u(k+1|k)^T& \dots  u(k+p-1|k)^T
	\end{matrix}
\right]^T
$$
我们可以通过离散化的状态方程来对未来$p$个控制周期的状态进行预测：
$$
x(k|k) = x(k)\\
x(k+1|k) = \Phi x(k)+Gu(k|k)\\
x(k+2|k) = \Phi^2 x(k) + \Phi Gu(k|k) + Gu(k+1|k)\\
\vdots\\
x(k+p|k) = \Phi ^ px(k) + \sum^{p-1}_{i=0}\Phi p-1-iGu(k+i|k)
$$
其中第一项$x(k|k)$就是当前系统的观测，或者称之为反馈。将其写为矩阵形式，则有：
$$
X_{k+1} = \Psi x(k) + \Theta U_k
$$
其中有：
$$
\Psi = 
\left[
	\begin{matrix}
		I\\
		\Phi \\
		\Phi^2 \\
		\vdots\\
		\Phi^p
	\end{matrix}
\right]

,
\Theta = 
\left[
	\begin{matrix}
		0\\
		\Phi\\
		\Phi G& G\\
		\vdots & & \ddots\\
		\Phi^{p-1}G&\Phi^{p-2}G&\dots& G
	\end{matrix}
\right]
$$
需要说明的是$\Theta$阵为下三角阵，反映了系统在时间上的因果关系，即$k+1$时刻的输入对$k$时刻的输出没有影响。

#### 3.2.2 优化

我们的控制目标是系统的状态跟踪一条轨迹，通常称为参考值。我们定义预测时域的参考值系列为：
$$
R_k = 
\left[
	\begin{matrix}
		r(k+1)^T & r(k+2)^T & \dots & r(k+p)^T
	\end{matrix}
\right]^T
$$
需要注意的是，在$k$时刻进行控制时，控制器就必须已经拿到$k \to k+p$时刻的参考值了，这对控制造成些阻碍。此处为了简化问题，我们认为参考值是$0$，即希望系统能回归到原点。我们希望的是寻找最佳的控制量$U_k$使得预测时域内的状态向量的变化与参考值越接近越好，这是之前所说的开源优化问题。为此我们需要设计一个优化目标函数：
$$
J(U_k) = X_k^TQX_k
$$
往往我们不希望控制的输出过大，还可以增加对控制量的约束：
$$
J(U_k) = (X_k - R_k)^TQ(X_k - R_k) + U_k^TWU_k
$$
此时的代价函数中$X_k和$$U_k$均为变量，而我们的优化目标是获得$U_k$的值，因此需要将$X_k$消去。将之前推导的预测的状态变量方程带入。并优化函数展开并合并同类项后，有：
$$
J\left( U_k \right)

\\ =\left( X_k-R_{_k} \right) ^TQ\left( X_k-R_{_k} \right) +U_{k}^{T}WU_k

\\ =\left( \overset{E}{\overbrace{\varPsi x\left( k \right) -R_k}}+\varTheta U_k \right) ^TQ\left( \varPsi x\left( k \right) +\varTheta U-R_{_k} \right) +U_{k}^{T}WU_k

\\ =\left( E+\varTheta U_k \right) ^TQ\left( E+\varTheta U \right) +U_{k}^{T}WU_k

\\ =E^TQE+\left( \varTheta U_k \right) ^TQ\left( \varTheta U_k \right) +2E^TQ\left( \varTheta U_k \right) +U_{k}^{T}WU_k

\\ =U_{k}^{T}\varTheta ^TQ\varTheta U_k+U_{k}^{T}WU_k+2E^TQ\varTheta U_k+E^TQE

\\ =U_{k}^{T}\left( \varTheta ^TQ\varTheta +W \right) U_k+\left( 2E^TQ\varTheta \right) U_k+E^TQE
$$
其中$E = \Psi x(k) - R_k$，且$E^TQE$是常数项，对取极值没有影响，因此直接舍去。

那么我们优化的目标就是：
$$
J\left( U_k \right) = U_{k}^{T}\left( \varTheta ^TQ\varTheta +W \right) U_k+\left( 2E^TQ\varTheta \right) U_k
$$
取得最小值。这里就可以看到MPC相比于LQR优秀的一点在于，在这个优化步骤中我们可以对系统的输出$u_k$做约束。上式中已经将优化函数化为了二次规划的一般形式.

### 3.3 一个例子

对于一个实际的系统：
$$
\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]
u(k)
$$
类似的，我们引入
$$
X(k) = 
\left [
	\begin{matrix}	
		x(k|k)^T \\ x(k+1|k)^T \\ x(k+2|k)^T\\ \vdots \\ x(k+p|k)^T
	\end{matrix}
\right]
,
U(k) = 
\left [
	\begin{matrix}	
		u(k|k)^T \\ u(k+1|k)^T \\ u(k+2|k)^T\\ \vdots \\ u(k+p-1|k)^T
	\end{matrix}
\right]
,
\Psi = \left[    \begin{matrix}        I\\        \Phi \\        \Phi^2 \\        \vdots\\        \Phi^p    \end{matrix}\right],\Theta = \left[    \begin{matrix}        0\\        \Phi\\        \Phi G& G\\        \vdots & & \ddots\\        \Phi^{p-1}G&\Phi^{p-2}G&\dots& G    \end{matrix}\right]
\\
X(k+1) = \Psi X(k) + \Theta U(k)
$$
带入这个例子中的$\Phi$和$G$，我们预测未来的三项，即$p=3$，则有：
$$
X(k) = 
\left [
	\begin{matrix}	
		x_1(k|k)^T\\
		x_2(k|k)^T\\	
		x_1(k+1|k)^T\\
		x_2(k+1|k)^T\\	
		x_1(k+2|k)^T\\
		x_2(k+2|k)^T\\	
		x_1(k+3|k)^T\\
		x_2(k+3|k)^T\\
	\end{matrix}
\right]

U(k) = 
\left [
	\begin{matrix}	
		u(k|k)^T \\ u(k+1|k)^T \\ u(k+2|k)^T
	\end{matrix}
\right]
$$

$$
\Psi =
\left [
	\begin{matrix}	
		1 &0\\
		0 &1\\
		1&0.1\\
		0&2\\
		1&0.3\\
		0&4\\
		1&0.7\\
		0&8\\
	\end{matrix}
\right]
,
\Theta= \left [
	\begin{matrix}	
		0 & 0& 0\\
		0 & 0& 0\\
		0 & 0& 0\\
		0.5& 0& 0\\
		0.05 & 0.5&0\\
		1 & 0&0\\
		0.15 & 0 & 0\\
		2 & 1 & 0.5
	\end{matrix}
\right]
$$

那么我们可以写出在系统趋向于0时的代价函数。但是写起来太麻烦了：

## 参考

### 教程
* [状态空间方程的离散化](https://zhuanlan.zhihu.com/p/326930724)
* [LQR控制器— 线性二次型调节器 Linear Quadratic Regulator](https://zhuanlan.zhihu.com/p/139145957)
* [模型预测控制（MPC）解析（一）](https://www.guyuehome.com/33800)
* [模型预测控制（MPC）简介及matlab实现](https://www.guyuehome.com/42496)
* [基本模型预测控制（MPC）原理](https://blog.csdn.net/grandpadzb/article/details/118937285)
* [一个模型预测控制（MPC）的简单实现](https://zhuanlan.zhihu.com/p/141871796)
    * https://github.com/YuXianYuan/model-predictive-control-demo

### 代码
* https://gitee.com/aircraft-is-design/TinyMPC
* https://gitee.com/aircraft-is-design/mpc-reinforcement-learning
* https://gitee.com/aircraft-is-design/PythonRobotics/blob/master/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
    * https://gitee.com/aircraft-is-design/PythonRoboticsGifs/blob/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif
* https://gitee.com/aircraft-is-design/CppRobotics/blob/master/src/model_predictive_control.cpp

飞行器智能感知与控制/learn_control

简介

发行版

贡献者

近期动态

飞行器智能感知与控制/learn_control .gitee-modal { width: 500px !important; }

简介

发行版

贡献者

近期动态

搜索帮助

飞行器智能感知与控制/learn_control