Apollo学习笔记（7）车辆运动学模型

这里又整了一遍车辆运动学，之前在Autoware那会，已经在控制那一部分整过一次了，就当复习了吧。

前言

车辆的控制目前用的最广的有三种方式：

1. PID
2. LQR
3. MPC

其中，PID是一种对模型无要求的控制算法，就算丝毫不了解其控制模型，都可以凭经验将其参数调试出来，并且保证其可靠性与精度。
另外两种控制算法，要想实现对车辆的控制就必须了解，车辆的运动学或者动力学模型，在控制专栏下，也有描述过使用的相关模型，但是不同的人使用的控制方式不一样，建立的模型也不一致，这里主要是针对Apollo中的相关模型。
Apollo中，计算控制命令需要车辆的定位信息、底盘信息以及规划信息等，具体的可以直接去看代码，这里就不赘述了。

单车模型

建立模型时，应尽可能使模型简单易用，且能真实反映车辆特性，现在使用较多的车辆运动学模型多基于单车模型，使用此模型要做出如下假设：

1. 不考虑车辆在Z轴方向的运动，只考虑XY水平面的运动，如图1所示；
2. 左右侧车轮转角一致，这样可将左右侧轮胎合并为一个轮胎，以便于搭建单车模型，如图2所示；
3. 车辆行驶速度变化缓慢，忽略前后轴载荷的转移；
4. 车身及悬架系统是刚性的。

$$图1车辆模型$$

单车模型(Bicycle Model)将左/右前轮合并为一个点，位于A点；将左/右后轮合并为一个点，位于B点；点C为车辆质心点。

$$图2单车模型$$

其中，$O$为OA、OB的交点，是车辆的瞬时滚动中心，线段OA、OB分别垂直于两个滚动轮的方向；$\beta$为滑移角（Tire Slip Angle），指车辆速度方向和车身朝向两者间所成的角度，$\psi$为航向角（Heading Angle），指车身与X轴的夹角。

符号	定义	符号	定义
A	前轮中心	B	后轮中心
C	车辆质心	O	转向圆心
V	质心车速	R	转向半径
${\ell }_r$	后悬长度	${\ell }_f$	前悬长度
$\beta$	滑移角	$\psi$	航向角
${\delta }_r$	后轮偏角	${\delta }_f$	前轮偏角

当车辆为前轮驱动时，后轮偏角${\delta }_r$恒为0。

车辆运动学模型

运动学是从几何学的角度研究物体的运动规律，包括物体在空间的位置、速度等随时间而产生的变化，因此，车辆运动学模型应该能反映车辆位置、速度、加速度等与时间的关系。在车辆轨迹规划过程中应用运动学模型，可以使规划出的轨迹更切合实际，满足行驶过程中的运动学几何约束，且基于运动学模型设计出的控制器也能具有更可靠的控制性能。

基于单车模型，如图2所示，搭建车辆运动学模型。由正弦法则，有：
$$\frac{\sin ({\delta }_f-\beta )}{{\ell }_f}=\frac{\sin (\pi /2-{\delta }_f)}{R}\text{\hspace{1em}(1)}$$
$$\frac{\sin (\beta -{\delta }_r)}{{\ell }_r}=\frac{\sin (\pi /2+{\delta }_r)}{R}\text{\hspace{1em}(2)}$$
将式(1)，(2)展开，有
$$\frac{\sin {\delta }_f\cos \beta -\sin \beta \cos {\delta }_f}{{\ell }_f}=\frac{\cos {\delta }_f}{R}\text{\hspace{1em}(3)}$$
$$\frac{\cos {\delta }_r\sin \beta -\cos \beta \sin {\delta }_r}{{\ell }_r}=\frac{\cos {\delta }_r}{R}\text{\hspace{1em}(4)}$$
结合式(3)，(4)有
$$(\tan {\delta }_f-\tan {\delta }_r)\cos \beta =\frac{{\ell }_f+{\ell }_r}{R}\text{\hspace{1em}(5)}$$
低速环境下，车辆行驶路径的转弯半径变化缓慢，此时可以假设车辆的方向变化率等于车辆的角速度。则车辆的横摆角速度为
$$\dot{\psi }=\frac{V}{R}\text{\hspace{1em}(6)}$$
联立式(5)，(6)有
$$\dot{\psi }=\frac{V\cos \beta }{{\ell }_f+{\ell }_r}(\tan {\delta }_f-\tan {\delta }_r)\text{\hspace{1em}(7)}$$
则在惯性坐标系下，有车辆运动学模型：
$$\{\begin{array}{l}\dot{X}=V\cos (\psi +\beta )\\ \dot{Y}=V\sin (\psi +\beta )\\ \dot{\psi }=\frac{V\cos \beta }{{\ell }_f+{\ell }_r}(\tan {\delta }_f-\tan {\delta }_r)\end{array}\text{\hspace{1em}(8)}$$
滑移角$\beta$可以由公式(3)，(4)求得：
$$\beta ={\tan }^{-1}(\frac{{\ell }_f\tan {\delta }_r+{\ell }_r\tan {\delta }_f}{{\ell }_f+{\ell }_r})\text{\hspace{1em}(9)}$$
可以看出模型的输入参数有三个$V$,${\delta }_f$,${\delta }_r$。
一般的车辆只有前轮转角${\delta }_f$，后轮转角${\delta }_r=0$，因此，$\beta$也近似为0，这样就与之前的文章中另一种推导方式结果相同。

阿克曼模型

在单车模型中，将转向时左/右前轮偏角假设为同一角度，虽然通常两个角度大致相等，但实际并不是，通常情况下，内侧轮胎转角更大。
如下图所示，${\delta }_o$​和${\delta }_i$​分别为外侧前轮和内侧前轮偏角，当车辆右转时，右前轮胎为内侧轮胎，其转角${\delta }_i$​​较左前轮胎转角${\delta }_o$更大。${\ell }_w$​为轮距，$L$为轴距，后轮两轮胎转角始终为0°。
当以后轴中心为参考点时，转向半径$R$为下图中红线。

当滑移角$\beta$很小，后轮偏角为0时（其实前轮转向车辆大部分时候都这样）公式(7)可以转换为：
$$\frac{\dot{\psi }}{V}\approx \frac{1}{R}=\frac{\delta }{L}\text{\hspace{1em}(10)}$$
由于内外侧轮胎的转向半径不同，因此有：
$${\delta }_o=\frac{L}{R+{\ell }_w/2}\text{\hspace{1em}(11)}$$
$${\delta }_i=\frac{L}{R-{\ell }_w/2}\text{\hspace{1em}(12)}$$
则前轮平均转角
$$\delta =\frac{{\delta }_o+{\delta }_i}{2}\approx \frac{L}{R}\text{\hspace{1em}(13)}$$
内外转角之差
$$\Delta \delta ={\delta }_i-{\delta }_o=\frac{L}{R^2}{\ell }_w={\delta }^2\frac{{\ell }_w}{L}\text{\hspace{1em}(14)}$$
可以看出，两个前轮的转向角之差$\Delta \delta$与平均转向角$\delta$的平方成正比。

依据阿克曼转向几何设计的车辆，沿着弯道转弯时，利用四连杆的相等曲柄使内侧轮的转向角比外侧轮大大约2~4度，使四个轮子路径的圆心大致上交会于后轴的延长线上瞬时转向中心，让车辆可以顺畅的转弯。

$$图4梯形拉杆装置差动转向机构$$