MPC（3）常用车辆模型

上一篇中介绍了，模型预测控制的理论方面基础，无人驾驶车辆采用MPC控制方法来进行相关控制的时候，就考虑结合车辆的运动学以及动力学的相关约束。而且，如果在路径规划阶段(Local Planner)就考虑到车辆运动学和动力学约束，则车辆运动跟踪性能会更好。这篇文章就主要介绍的就是，前轮驱动小轿车目前常用的几种车辆运动学以及动力学模型。

阿克曼模型

车辆低速行驶时，轮胎不会产生侧滑，同时，车辆转向时，左前轮和右前轮偏转的角度是不同的，如下图所示（画图很麻烦，所以图片很多都是网上找的）

由于两个前轮的转向角度不同，造成了，两个后轮旋转半径也不相同，于是，做出了相应的简化，以车辆后轴中心点为旋转参考点，对车辆左前轮角度和右前轮角度取均值：

$$\delta =\frac{1}{2}({\delta }_l+{\delta }_r)\text{\hspace{1em}(1)}$$
式中，$\delta$为简化后车辆前轮转向角，${\delta }_l$和${\delta }_r$分别为对应的车辆左侧前轮角度和右侧前轮转向角度。简化后的模型，称为自行车模型，自行车模型最大的好处就是把前轮转角与车辆后轴质心旋转的半径联系了起来。
$$tan\delta =\frac{L}{R}\text{\hspace{1em}(2)}$$
其中，$\delta$ 为车辆前轮转角
$L$ 为车辆前后轴之间的距离(Wheel Base)
$R$ 为车辆在给定的前轮转角$\delta$情况下，所遵循的圆轨迹的半径。

车辆运动学模型

如下图所示，车辆转向模型：

在惯性坐标系XOY下，$(X_f,Y_f)$和$(X_r,Y_r)$分别代表车辆后轴中心和前轴中心的坐标，$\phi$表示车辆的航向角，${\delta }_f$表示前轮偏角，$v_r$表示车辆后轴中心沿车辆轴线方向的速度，$v_f$表示车辆前轴中心沿着前轮偏角方向的速度，$l$表示车辆前后轴之间的距离。
在车辆后轴中心$(X_r,Y_r)$处，有
$$v_r={\dot{X}}_r\ast cos(\phi )+{\dot{Y}}_r\ast sin(\phi )\text{\hspace{1em}(3)}$$
前、后轴的运动学约束（车辆无横向侧滑）：
$$\{\begin{array}{l}{\dot{X}}_f\ast sin(\phi +{\delta }_f)-{\dot{Y}}_f\ast cos(\phi +{\delta }_f)=0\\ {\dot{X}}_r\ast sin(\phi )-{\dot{Y}}_r\ast cos(\phi )=0\end{array}\text{\hspace{1em}(4)}$$
根据式（3）、式（4）可以得出：
$$\{\begin{array}{l}{\dot{X}}_r=v_r\ast cos(\phi )\\ {\dot{Y}}_r=v_r\ast sin(\phi )\end{array}\text{\hspace{1em}(5)}$$

根据前后轮的位置关系有：
$$\{\begin{array}{l}{X}_f=X_r+l\ast cos\phi \\ Y_f=Y_r+l\ast sin\phi \end{array}\text{\hspace{1em}(6)}$$

将（5）、（6）带入（4）可以得出横摆角$\omega$速度为：
$$\dot{\phi }=\omega =\frac{v_r}{l}\ast tan({\delta }_f)\text{\hspace{1em}(7)}$$

由横摆角$\omega$和车速$v_r$可以计算出车辆在不同的前轮转向角的条件下，对应的不同转向半径(可以看出与阿克曼模型的转弯半径计算方式相同)：
$$\{\begin{array}{l}R=v_r\ast \omega \\ {\delta }_f=arctan(\frac{l}{R})\end{array}\text{\hspace{1em}(8)}$$
由式（5），（7）可以得出车辆运动学模型
$$\left[\begin{array}{c}{\dot{X}}_r\\ {\dot{Y}}_r\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}cos\phi \\ sin\phi \\ tan({\delta }_f)/l\end{array}\right]\ast v_r\text{\hspace{1em}(9)}$$
车辆的可控制量为$v_r$和${\delta }_f$,而在实际使用中，最好的方式是希望以$[v_r,\omega ]$作为控制量，将式（7）带入式（9）中，可以将车辆运动学模型可以被转换为以下形式：
$$\left[\begin{array}{c}{\dot{X}}_r\\ {\dot{Y}}_r\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}cos\phi \\ sin\phi \\ 0\end{array}\right]\ast v_r+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\ast \omega \text{\hspace{1em}(10)}$$
由车辆运动学模型可以看作是对阿克曼运动模型的延伸，通过运动学方程可以计算出控制时域内，可以计算出车身后轴中心点坐标$X$，$Y$以及航向角$\phi$在预测时间内的变化量。

车辆动力学模型

在进行车辆动力学建模时，首先要进行以下理想化假设：

• 假设车辆在平坦路面上行驶，忽略车辆的垂向运行
• 忽略悬架系统运动造成的相关影响
• 只考虑纯侧偏轮胎特性，忽略轮胎力的纵横向耦合关系
• 忽略车辆前后以及左右向的载荷转移
• 忽略纵向横向的空气动力学
  同样，设定车辆为前轮驱动，以车辆质心为原点$O$,沿着车辆纵向轴线指向车头方向为$X$轴正向，垂直于车辆，车身水平面内垂直于$X$轴且指向车身左侧方向为$Y$轴，$Z$轴垂直于$XY$面且垂直向上。具体如下图所示：

关于其中轮胎受力定义如下：
$F_{lf},F_{lr}$ —— 前、后轮胎受到的纵向力
$F_{cf},F_{cr}$ —— 前、后轮胎受到的侧向力
$F_{xf},F_{xr}$ —— 前、后轮胎受到的x方向的力
$F_{yf},F_{yr}$ —— 前、后轮胎收到的y方向的力

根据牛顿第二定律，分别可得沿$x$轴，$y$轴和绕$z$轴的受力平衡方程
在$X$轴方向上：
$$m\ast \ddot{x}=m\ast \dot{y}\ast \dot{\phi }+2\ast F_{xf}+2\ast F_{xr}\text{\hspace{1em}(11)}$$

在$Y$轴方向上：
$$m\ast \ddot{y}=-m\ast \dot{x}\ast \dot{\phi }+2\ast F_{yf}+2\ast F_{yr}\text{\hspace{1em}(12)}$$

绕$Z$轴方向：
$$I_z\ast \ddot{\phi }=2\ast a\ast F_{yf}-2\ast b\ast F_{yr}\text{\hspace{1em}(13)}$$
式中，$a$和$b$分别为质心到前、后轴的距离，$m$为车辆装备质量，$I_z$为车辆绕$z$轴的转动惯量。

轮胎在$x,y$方向上受到的合力与纵向、侧向力的转换关系如下：
$$\{\begin{array}{l}{F}_{xf}=F_{lf}\ast cos({\delta }_f)-F_{cf}\ast sin({\delta }_f)\\ F_{xr}=F_{lr}\ast cos({\delta }_r)-F_{cr}\ast sin({\delta }_r)\\ F_{yf}=F_{lf}\ast sin({\delta }_f)+F_{cf}\ast cos({\delta }_f)\\ F_{yr}=F_{lr}\ast sin({\delta }_r)+F_{cr}\ast cos({\delta }_r)\end{array}\text{\hspace{1em}(14)}$$
轮胎的纵向力、侧向力可以表示为轮胎的侧偏角、滑移率、路面摩擦系数和垂向载荷等参数的复杂函数：
$$\{\begin{array}{l}{F}_l=f_l(\alpha ,s,\mu ,F_z)\\ F_c=f_c(\alpha ,s,\mu ,F_z)\end{array}\text{\hspace{1em}(15)}$$

式中，$\alpha$为轮胎侧偏角，$s$为滑移率，$\mu$为路面摩擦系数，$F_z$为轮胎所受到的垂向载荷。

轮胎的侧偏角$\alpha$可以由几何关系计算得到：
$$\alpha =arctan\frac{v_c}{v_l}\text{\hspace{1em}(16)}$$

式中，$v_c$与$v_l$为轮胎在侧向、纵向的速度，可以用坐标系方向的速度$v_x$和$v_y$表示：
$$\{\begin{array}{l}{v}_l=v_y\ast sin\delta +v_x\ast cos\delta \\ v_c=v_y\ast cos\delta -v_x\ast sin\delta \end{array}\text{\hspace{1em}(17)}$$

式中，$\delta$为轮胎偏转角。

轮胎的速度获取通常比较麻烦，现在的车子四个轮子在旋转时会发出相应的脉冲信号，通过解析这些信号获取四个轮子的轮速是一种很方便且精确的方式，另外一种就是通过车辆速度计算得出，这种方法虽说没有上一种方式精确，但是使用范围广泛，具体公式如下：
$$\begin{array}{c}{v}_{yf}=\dot{y}+a\ast \dot{\phi }\\ v_{yr}=\dot{y}-b\ast \dot{\phi }\\ v_{xf}=\dot{x}\\ v_{xr}=\dot{x}\end{array}\text{\hspace{1em}(18)}$$

轮胎在地面上的滑移率$s$的计算公式：
$$s=\{\begin{array}{ll}\frac{r\ast w_t}{v}-1& \text{if}v>r\ast w_t,v\ne 0\\ 1-\frac{v}{r\ast w_t}& \text{if}v<r\ast w_t,w_t\ne 0\end{array}\text{\hspace{1em}(19)}$$

式中，$r$为车轮半径，$w_t$为车轮旋转角速度。
车辆前后轴所受的垂向载荷：
$$\{\begin{array}{l}{F}_{zf}=\frac{b\ast m\ast g}{2\ast (a+b)}\\ F_{zr}=\frac{a\ast m\ast g}{2\ast (a+b)}\end{array}\text{\hspace{1em}(20)}$$

最后，考虑车身坐标系以及惯性坐标系之间的关系有：
$$\{\begin{array}{l}\dot{Y}=\dot{x}\ast sin\phi +\dot{y}\ast cos\phi \\ \dot{X}=\dot{x}\ast cos\phi -\dot{y}\ast sin\phi \end{array}\text{\hspace{1em}(21)}$$

以上就是车辆非线性动力学模型。路面摩擦系数$\mu$为路面固有信息，滑移率$s$的控制需要由另一套控制系统来控制（ABS），在此我们假设此控制系统已经很好，滑移率$s$就是一个已知量。

总结

以上三个模型都是常用的车辆模型，但是每个都有不同的用处以及不足，实际应用运动学和动力学要求都必须考虑到，当然，车速很低的情况下，需要考虑的因素较少，实践出真理，现有的MPC控制算法证明已经是可行的方案，唯一的缺陷就是MPC一旦预测步长过多，对算力的要求比较高。因此，模型的简化和算力之间的平衡，需要实际情况实际考虑，在此就不赘述了。