四、车辆横向控制（基于几何）

1、车辆横向运动学模型

1.1、自行车模型(二自由度模型)

车辆运动学模型用于描述车辆在二维平面上的运动能力。在车速不高的情况下，自行车模型已能够符合实际需求，该模型遵循以下几点假设：

（1）车辆行驶于平坦路面，忽略车辆在垂直于路面方向上的运动；

（2）忽略车辆在行驶中受到的空气阻力以及地面侧向摩擦力；

（3）车轮与地面始终保持良好的滚动接触；

（4）车辆为刚体，忽略车身悬架结构的影响；

（5）左右两前轮、两后轮的转向角相等；

惯性坐标系下的自行车模型如上图所示。

$\psi$为车辆横摆角，即车辆的纵轴$\vec{x}$与$\vec{X}$轴之间的夹角（绝对坐标系）；

$\beta$为质心侧偏角，即车辆的纵轴$\vec{x}$与质心速度的夹角（车身坐标系）；

${\delta }_f$与${\delta }_r$分别为前轮转角与后轮转角（一般为0）；

$l_f$与$l_r$分别为质心距前轴和后轴的距离；

在建立系统的状态空间表达式前，可以确定系统的控制量和状态量。

控制量：

前轮转角${\delta }_f$（方向盘转角）；

车身加速度$a$（油门以及刹车信号）；

状态量（能够完全表征系统运动状态的一组最小变量）：

车辆位置$(x,y)$；

速度$v$、加速度$a$;

横摆角$\psi$、横摆角速度$\dot{\psi }$；

根据不同的建模需求，可以选择不同的状态量。

如图所示，首先需要找到车辆此时的瞬时转动中心$O$，根据几何关系，可以列写方程如下：
{ sin ⁡ ( δ f − β ) l f = sin ⁡ ( π 2 − δ f ) R sin ⁡ ( β − δ r ) l r = sin ⁡ ( π 2 + δ r ) R

$$\begin{cases} \frac{\sin(\delta_f-\beta)}{l_f}=\frac{\sin(\frac{\pi}{2}-\delta_f)}{R}\\ \frac{\sin(\beta-\delta_r)}{l_r}=\frac{\sin(\frac{\pi}{2}+\delta_r)}{R} \end{cases}$$ {lf​sin(δf​−β)​=Rsin(2π​−δf​)​lr​sin(β−δr​)​=Rsin(2π​+δr​)​​

将上式展开，得：
{ sin ⁡ ( δ f ) cos ⁡ ( β ) − sin ⁡ ( β ) cos ⁡ ( δ f ) l f = cos ⁡ ( δ f ) R cos ⁡ ( δ r ) sin ⁡ ( β ) − cos ⁡ ( β ) sin ⁡ ( δ r ) l r = cos ⁡ ( δ r ) R

$$\begin{cases} \frac{\sin(\delta_f)\cos(\beta)-\sin(\beta)\cos(\delta_f)}{l_f}=\frac{\cos(\delta_f)}{R}\\ \frac{\cos(\delta_r)\sin(\beta)-\cos(\beta)\sin(\delta_r)}{l_r}=\frac{\cos(\delta_r)}{R} \end{cases}$$ {lf​sin(δf​)cos(β)−sin(β)cos(δf​)​=Rcos(δf​)​lr​cos(δr​)sin(β)−cos(β)sin(δr​)​=Rcos(δr​)​​
继续化简，得：
$$\{\begin{array}{l}\tan ({\delta }_f)\cos (\beta )-\sin (\beta )=\frac{l_f}{R}\\ \sin (\beta )-\tan ({\delta }_r)\cos (\beta )=\frac{l_r}{R}\end{array}$$

将方程组中两式相加，得：
$$(\tan ({\delta }_f)-tan({\delta }_r))\cos (\beta )=\frac{l_f+l_r}{R}$$
在低速情况下，可以认为车辆横摆角速度$\dot{\psi }$等于车辆质心角速度，即：
ψ ˙ ≈ r = V R = V ⋅ 1 R = V ( tan ⁡ ( δ f ) − tan ⁡ ( δ r ) ) cos ⁡ ( β ) l f + l r = V cos ⁡ ( β ) l f + l r ( tan ⁡ ( δ f ) − t a n ( δ r ) )

$$\begin{aligned} \dot{\psi}\approx r=\frac{V}{R}=V\cdot \frac{1}{R}&=V\frac{(\tan(\delta_f)-\tan(\delta_r))\cos(\beta)}{l_f+l_r}\\ &=\frac{V\cos(\beta)}{l_f+l_r}(\tan(\delta_f)-tan(\delta_r)) \end{aligned}$$ ψ˙​≈r=RV​=V⋅R1​​=Vlf​+lr​(tan(δf​)−tan(δr​))cos(β)​=lf​+lr​Vcos(β)​(tan(δf​)−tan(δr​))​
同样，由
$$\{\begin{array}{l}\tan ({\delta }_f)\cos (\beta )-\sin (\beta )=\frac{l_f}{R}\\ \sin (\beta )-\tan ({\delta }_r)\cos (\beta )=\frac{l_r}{R}\end{array}$$
可以推出
$$\{\begin{array}{l}{l}_r\times (\tan ({\delta }_f)\cos (\beta )-\sin (\beta ))=\frac{l_f{l}_r}{R}\\ l_f\times (\sin (\beta )-\tan ({\delta }_r)\cos (\beta ))=\frac{l_f{l}_r}{R}\end{array}$$
则质心侧偏角$\beta$的表达式为
$$\beta ={\tan }^{-1}(\frac{l_f\tan {\delta }_r+l_r\tan {\delta }_f}{l_f+l_r})$$
综上，即得到以质心为参考点的车辆运动学模型
$$\{\begin{array}{l}\dot{X}=V\cos (\psi +\beta )\\ \dot{Y}=V\sin (\psi +\beta )\\ \dot{\psi }=\frac{V\cos (\beta )}{l_f+l_r}(\tan ({\delta }_f)-tan({\delta }_r))\\ \beta ={\tan }^{-1}(\frac{l_f\tan {\delta }_r+l_r\tan {\delta }_f}{l_f+l_r})\end{array}$$

1.2、阿克曼转向几何

阿克曼转向几何（Ackermann steering geometry）是一种为了解决交通工具转弯时，内外转向轮路径指向的圆心不同的几何学。

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

汽车的横摆角速度为$\dot{\psi }=\frac{V\cos (\beta )}{l_f+l_r}(\tan ({\delta }_f)-tan({\delta }_r))$；

假设后轮转角${\delta }_r$为0；前轮转角很小，则$\tan ({\delta }_f)$近似等于${\delta }_f$；质心侧偏角$\beta$很小，则$\cos (\beta )$近似等于1；车辆轴距$L=l_f+l_r$。

则横摆角速度$\dot{\psi }=\frac{V}{R}=V\frac{\delta }{L}$。

由此可以得到左右两轮平均转角表达式${\delta }_{ave}=\frac{L}{R}$；

也可以近似得到外侧车轮与内侧车轮转向角表达式（假设转向半径$R$远远大于轮距$l_w$）：

外侧车轮转向角${\delta }_o=\frac{L}{R+l_w/2}$；

内侧车轮转向角${\delta }_i=\frac{L}{R-l_w/2}$；

内外侧转向角的差值${\delta }_i-{\delta }_o=\frac{L{l}_w}{R^2-l_w^2/4}$；

同样的，由于转向半径$R$远远大于轮距$l_w$，可以把$l_w^2/4$项忽略；

则${\delta }_i-{\delta }_o\approx \frac{L\cdot l_w}{R^2}={\delta }^2\frac{l_w}{L}$

由此可以看出，在相同情况下，转角越大，内外侧车轮转角差值越大，这就是阿克曼转向几何。

2、纯追踪法

2.1、算法原理

纯跟踪算法（Pure Pursuit）是一种传统且经典的车辆横向运动控制算法，其基本思想是在每个控制周期，通过前方目标轨迹上的一个点，指导当前方向盘的动作，使车辆车产生向目标点的运动。

纯追踪算法基于自行车模型和阿克曼转向几何。如图所示， 假设前轮转角为$\delta$，后轮划过的半径为$R$，车辆轴距为$L$，则有$\tan \delta =\frac{L}{R}$，该式能在车速较低的场景下对车辆的运动情况作简单的估计。

从自行车模型出发，纯跟踪算法以车后轴为切点, 车辆纵向车身为切线, 通过控制前轮转角 ， 使车辆可以沿着一条经过目标路点的圆弧行驶，如下图所示：

根据三角形正弦定理，可知

$\frac{l_d}{\sin (2\alpha )}=\frac{R}{\sin (\frac{\pi }{2}-\alpha )}\Rightarrow \frac{l_d}{2\sin (\alpha )\cos (\alpha )}=\frac{R}{\cos (\alpha )}\Rightarrow \frac{l_d}{\sin (\alpha )}=2R$

由上述两式可以推出$\delta (t)={\tan }^{-1}(\frac{2L\sin (\alpha (t))}{l_d})$

在等腰三角形中，又有$\sin (\alpha )=\frac{e_{ld}}{l_d}$（$e_{ld}$为横向误差）

继续代入，可以推出$\delta (t)={\tan }^{-1}(\frac{2L}{l_d^2}{e}_{ld}(t))$

由此，就得到了前轮转角$\delta$与前视横向误差$e_{ld}$、轴距$L$、前视距离$l_d$之间的关系。

2.2、实现步骤

（1）确定车辆当前的位置（定位模块）；

（2）找到规划路径上距离当前车辆最近的点$(g_{xmin},g_{ymin})$(为下一步作准备)；

（3）找到目标点$(g_x,g_y)$；首先定义好前视距离$l_d$，以汽车后轮接地点为中心，以前视距离$l_d$为半径画圆，圆弧将会与规划路径交于一点或者多点，则与（2）中定义的最近点$(g_{xmin},g_{ymin})$距离最近的点即为目标点；

（4）将目标点转换到车辆坐标系下；

（5）利用纯跟踪算法的公式计算出到达目标点所需要的转角，并控制车辆进行转向；

（6）更新车辆实时状态；

可以看到，纯追踪算法的核心在于前视距离$l_d$的选取，前视距离过小会使车辆行驶路径产生振荡，而过大则会导致车辆过弯时拐小弯。

一种比较常见的前视距离的选择方法是将其与速度相关联，令$l_d$是速度的一次函数，即$l_d=k{v}_x$，使用这种方法可以使控制适应速度的变化。

3、Stanley 法

3.1、算法原理

Stanley法又称为前轮反馈控制法（Front wheel feedback)，核心在于基于前轮中心的路径跟踪偏差量对方向盘转向控制量进行计算。

前轮转角$\delta$由需要考量三个条件：

（1）由航向误差引起的转角${\delta }_e$，即当前车身方向与参考轨迹最近的点的切线方向的夹角；

（2）由横向误差$e$引起的转角${\delta }_y$，在不考虑航向误差时，横向误差越大，前轮转角应该越大。设在轨迹切线方向前$d(t)$处轨迹切线与前轮方向线橡胶，根据几何关系，可以得到${\delta }_y={\tan }^{-1}\frac{e}{d(t)}={\tan }^{-1}\frac{ke}{v(t)}$，式中，$d(t)$与车速相关，即$d(t)=\frac{v(t)}{k}$，$k$为比例系数，可以根据实验来调整；

（3）前轮转角有最大值${\delta }_{max}$和最小值${\delta }_{min}$，通常${\delta }_{max}$和${\delta }_{min}$是一对相反数；

综上，可以得到前轮转角的表达式：

$\delta (t)={\delta }_e(t)+{\tan }^{-}1(\frac{ke(t)}{v(t)}),\delta (t)\in [{\delta }_{min},{\delta }_{max}]$

3.2、稳定性分析

使用线性自行车运动模型，可以得到横向误差的变化率为$\dot{e}(t)=-v(t)\sin {\delta }_e(t)$;

根据几何关系，得$sin{\delta }_e(t)=\frac{e(t)}{\sqrt{d(t{)}^2+e(t{)}^2}}=\frac{ke(t)}{\sqrt{v(t{)}^2+(ke(t){)}^2}}$；

因此，$\dot{x}(t)=\frac{-v(t)ke(t)}{\sqrt{v(t^2)+(ke(t){)}^2}}=\frac{-ke(t)}{\sqrt{1+(\frac{ke(t)}{v(t)}{)}^2}}$；

假设横向误差$e(t)$很小，则分子项约等于1，则$\dot{e}\approx -ke(t)$；

积分上式，得$e(t)=e(0)\times ex{p}^{-kt}$，因此横向误差指数收敛于$e(t)=0$，参数$k$决定了收敛速度，对于任意横向误差，微分方程都单调的收敛到0。