MPC原理解释以及代码实现
MPC原理解释以及代码实现
MPC（模型预测控制）是一种在算力资源充足的情况下，可以在线性/非线性系统中实现对系统的约束和预测控制的算法。它广泛应用于自动驾驶领域，能够实现高精度轨迹跟踪和复杂约束系统的控制。本文将从最优控制、代价函数、MPC基本概念等方面详细讲解MPC的工作原理和应用场景。
一.场景分析
假设一辆汽车在路面上行驶，想要变道。在正常情况下，汽车会采用较为舒缓的路线，这样在动力学上消耗较少：
然而，在紧急避障的情况下，比如前方突然出现一辆汽车，为了躲避，汽车需要紧急向另一车道进行变道，此时舒适度不再是考虑因素。这种情况下，汽车在输入能量上的消耗会比正常变道时更多：
二.最优控制和代价函数
在上述两种情况下，哪一种是更优控制呢？显然，图一在考虑舒适角度上更加优秀，而图二在紧急避障上则处理更加迅速。因此，对于不同的应用背景，应当设定不同的指标去衡量优劣，引入代价函数（Cost Function）的概念。
单输入单输出系统（SISO）
对于单输入单输出系统而言，衡量系统性能优劣可以用误差的积累值和输入的积累值来衡量。由此，我们可以定义代价函数：
$$
J = \sum_{k=0}^{N} (x(k) - x_d(k))^T Q (x(k) - x_d(k)) + u(k)^T R u(k)
$$
其中，$Q$ 和 $R$ 分别表示一个增益系数。如果 $Q$ 大，表示希望误差变得更小，收敛更快；$R$ 大，表示更注重输入累积，更注重节能。
多输入多输出系统（MIMO）
接下来，我们把其推广到多输入多输出系统（MIMO），使用状态空间描述为：
$$
\begin{align*}
x(k+1) &= Ax(k) + Bu(k) \
y(k) &= Cx(k) + Du(k)
\end{align*}
$$
这里 $x$ 是系统的状态变量，$y$ 是系统的输出量，$u$ 为系统的输入量。此时，衡量系统表现优劣就要引入二次型的知识，我们定义：
$$
J = \sum_{k=0}^{N} (x(k) - x_d(k))^T Q (x(k) - x_d(k)) + u(k)^T R u(k) + (x(N) - x_d(N))^T P (x(N) - x_d(N))
$$
其中，$P$ 是终端代价矩阵。
三.MPC 模型预测控制的基本概念
MPC 通过模型来预测系统在某一个时间段内的表现来进行优化控制。在 $k$ 时刻，我们可以测量或估计出系统的当前状态 $y(k)$，再通过计算得到的 $u(k), u(k+1), u(k+2), \ldots, u(k+j)$ 得到系统未来状态的估计值 $y(k+1), y(k+2), \ldots, y(k+j)$。我们将预测估计的部分称为预测区间（Predictive Horizon），将控制估计的部分称为控制区间（Control Horizon）。
总的步骤如下：
- 估计/测量读取当前系统状态量
- 基于 $u_k, u_{k+1}, \ldots, u_{k+n}$ 来进行最优化
- 通过上面的计算，我们可以得到 $u_k, u_{k+1}, u_{k+3}$ 等控制区间的控制量，但是我们在实际实行输入量时，只输入本时刻计算的 $u_k$，在每一个时间单位都进行预测（求解最优化问题），我们将这样的方案称为滚动优化控制（Receding Horizon Control）。
四.最优化建模
对于 MPC 而言，有很多中不同方法对其进行最优化，这里介绍较为常用的二次规划（Quadratic Programming）。
在一般的离散系统中，形式可以表示为：
$$
\begin{align*}
x(k+1) &= Ax(k) + Bu(k) \
y(k) &= Cx(k) + Du(k)
\end{align*}
$$
我们定义：
$$
\begin{align*}
U_k &= [u(k), u(k+1), \ldots, u(k+N-1)]^T \
X_k &= [x(k+1), x(k+2), \ldots, x(k+N)]^T
\end{align*}
$$
是 $k$ 时刻预测的输入值，而
$$
X_k = Mx(k) + CU_k
$$
是 $k$ 时刻预测的状态值，我们设：
对于期望输入为 0，输出向量等于状态向量的离散系统：
$$
J = \sum_{k=0}^{N} (x(k) - x_d(k))^T Q (x(k) - x_d(k)) + u(k)^T R u(k) + (x(N) - x_d(N))^T P (x(N) - x_d(N))
$$
其中，我们需要求解的是系统的输入 $u(k)$，这就需要我们把状态项 $x(k)$ 给消除掉，处理这个事情需要利用系统的状态方程，首先有
$$
x(k+1) = Ax(k) + Bu(k)
$$
我们可以通过传感器或者状态估计得到系统当前的状态值，这相当于系统的一个初值，由初值和状态方程可以得到其他项为：
$$
\begin{align*}
x(k+1) &= Ax(k) + Bu(k) \
x(k+2) &= A^2x(k) + ABu(k) + Bu(k+1) \
&\vdots \
x(k+N) &= A^Nx(k) + A^{N-1}Bu(k) + \cdots + Bu(k+N-1)
\end{align*}
$$
我们把它简单整理一下，有：
$$
X_k = Mx(k) + CU_k
$$
即：
$$
X_k = Mx(k) + CU_k
$$
上式还可根据之前推导的公式继续化简，其中，$M$ 与 $C$ 互为转置，但他们彼此又都是常数，所以他们彼此相等，因此有：
$$
X_k = Mx(k) + CU_k
$$
再令：
$$
H = C^TQC + R
$$
有：
$$
J = U_k^T H U_k + 2U_k^T C^TQ(Mx(k) - x_d(k)) + (Mx(k) - x_d(k))^T Q (Mx(k) - x_d(k))
$$
由此我们就得到了模型预测控制代价函数的简单形式。