差分轮移动机器人模型预测控制MPC

模型预测控制（MPC）与PID、纯追踪法相比有更好的路径跟踪效果，在自动驾驶领域有广泛应用。本文将以运动学为基础详细推导差分轮移动机器人模型预测控制（MPC）

运动学模型

根据移动机器人的运动学结构可得移动机器人的状态方程：
$$\begin{array}{l}\dot{x}=v\cos \theta \\ \dot{y}=\mathrm{vsin}\theta \\ \dot{\theta }=w\end{array}\text{\hspace{1em}(1)}$$
式（1）为非线性方程，不利于后面进行优化求解，便采用一阶泰勒公式将状态方程（1）进行线性化得：
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{c}{v}_r\cos {\theta }_r\\ v_r\sin {\theta }_r\\ w_r\end{array}\right]+\left[\begin{array}{ccc}0& 0& -v_r\sin {\theta }_r\\ 0& 0& v_r\cos {\theta }_r\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}x-x_r\\ y-y_r\\ \theta -{\theta }_r\end{array}\right]+\left[\begin{array}{cc}\cos {\theta }_r& 0\\ \sin {\theta }_r& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v-v_r\\ w-w_r\end{array}\right]\text{\hspace{1em}(2)}$$

其中，$x_r,y_r,{\theta }_r,v_r,w_r为移动机器人当前的状态。$为了方便后续书写，记：$X=\left[\begin{array}{l}x\\ y\\ \theta \end{array}\right]$，$A=\left[\begin{array}{ccc}0& 0& -v_{r}sin{\theta }_r\\ 0& 0& v_{r}cos\theta \\ 0& 0& 0\end{array}\right]$，$B=\left[\begin{array}{cc}cos{\theta }_r& 0\\ sin{\theta }_r& 0\\ 0& 1\end{array}\right]$，$O=-A\left[\begin{array}{c}{x}_r\\ y_r\\ {\theta }_r\end{array}\right]$，$u=\left[\begin{array}{c}v\\ w\end{array}\right]$得：
$$\dot{X}=AX+Bu+O\text{\hspace{1em}(3)}$$
为了得到移动机器人的位姿状态，就需要求解状态方程式（3），有多种方法求解式（3），本文只介绍使用一阶差分的方法求解状态方程得：
$$\frac{X_n-X_{n-1}}{T}=A{X}_{n-1}+B{u}_{n-1}+O\text{\hspace{1em}(4)}$$
其中$T$为控制周期，整理式(4)得：
$$X_n=(I+TA)X_{n-1}+TB{u}_{n-1}+TO\text{\hspace{1em}(5)}$$
至此，完成差分轮移动机器人数学模型的建立。由于存在近似化的过程，式（5）并不能精准预测移动机器人的状态，但线性化方程便于优化求解。

状态预测

本文以3个滚动时域为例介绍此过程。通过式（5）可知移动机器人在一系列输入$u_0,u_1,u_2$的作用下未来位姿的估计。
$$\begin{array}{c}{X}_1=(I+TA)X_0+TB{u}_0+TO\\ X_2=(I+TA)X_1+TB{u}_1+TO\\ X_3=(I+TA)X_2+TB{u}_2+TO\end{array}\text{\hspace{1em}(6)}$$
其中，$X_0$表示移动机器人当前的位置。MPC的目的是使移动机器人跟踪一条给定的路径，为此，应当计算出移动机器人预测位姿与路径的偏差，本文选择一个控制周期长度间隔的路径点$X_{ref1},X_{ref2},X_{ref3}$作为参考点计算偏差。
$$\begin{array}{c}{X}_1-X_{ref1}=(I+TA)X_0+TB{u}_0+TO-X_{ref1}\\ X_2-X_{ref2}=(I+TA)X_1+TB{u}_1+TO-X_{ref2}\\ X_3-X_{ref3}=(I+TA)X_2+TB{u}_2+TO-X_{ref3}\end{array}\text{\hspace{1em}(7)}$$
令$Y=\left[\begin{array}{c}{X}_1-X_{ref1}\\ X_2-X_{ref2}\\ X_3-X_{ref3}\end{array}\right]$，$U=\left[\begin{array}{c}{u}_0\\ u_1\\ u_2\end{array}\right]$，$\bar{O}=\left[\begin{array}{c}TO-X_{ref1}\\ (I+TA+I)TO-X_{ref2}\\ ((I+TA{)}^2+I+TA+I)TO-X_{ref3}\end{array}\right]$，$\bar{A}=\left[\begin{array}{c}I+TA\\ (I+TA{)}^2\\ (I+TA{)}^3\end{array}\right]$，$\bar{B}=\left[\begin{array}{ccc}TB& 0& 0\\ (I+TA)TB& TB& 0\\ (I+TA{)}^{2}TB& (I+TA)TB& TB\end{array}\right]$得：
$$Y=\bar{A}{X}_0+\bar{B}U+\bar{O}\text{\hspace{1em}(8)}$$

控制

为了达到一个良好的控制效果，需要定义一个指标描述控制的效果，即一个目标函数。直观上，移动机器人运行轨迹越贴近参考路径越好，这与偏差$Y$相关，而且机器人运行越平稳越好，即速度变化量小，这与输出量$U$相关。可以简要定义目标函数$J$:
$$\begin{array}{c}J=Y^{T}QY+U^{T}RU\\ m\le gU\le n\end{array}\text{\hspace{1em}(9)}$$
将式（8）带入式（9），并消去无关常数项，整理得：
$$\begin{array}{c}\underset{U}{\min }J=U^T({\bar{B}}^{T}Q\bar{B}+R)U+2(\bar{A}{X}_0+\bar{O}{)}^{T}Q\bar{B}U\\ m\le gU\le n\end{array}\text{\hspace{1em}(10)}$$
可以使用二次规划器对上述方程进行求解，如OSQP或matlab的quadprog函数，并将解的第一项$u_0$做为控制输出。

注意事项

1. 单纯的MPC控制在实际运行过程中，并不能完全保证优秀的路径跟踪能力，比如当路径的曲率变化比较大时，如果不减速处理，很容易导致移动机器人偏离路径。因此，参考点的选取也是一个关键的操作，从经验的角度来看，路径曲率变化较大的地方，移动机器人的线速度要比较小，此处路径段参考点之间的距离应小一点。
2. 由于运用运动学模型，因此，此MPC只适合低速下运行，无法应用与高速运行的情况，如无人驾驶。