自动驾驶控制算法——车辆运动学模型

车辆运动学模型

自行车模型(Bicycle Model)

自行车模型(Bicycle Model)的建立基于如下假设:

• 不考虑车辆在垂直方向(Z轴方向)的运动，即假设车辆的运动是一个二维平面上的运动;
• 假设车辆左右侧轮胎在任意时刻都拥有相同的转向角度和转速；这样车辆的左右两个轮胎的运动可以合并为一个轮胎来描述;
• 假设车辆行驶速度变化缓慢，忽略前后轴载荷的转移;
• 假设车身和悬架系统都是刚性系统;
• 假设车辆的运动和转向是由前轮驱动(front−wheel−only)的，即认为车辆的后轮转向角${\delta }_r=0$.

其中，O为OA、OB的交点，是车辆的瞬时滚动中心，线段OA、OB分别垂直于两个滚动轮的方向；V为质心车速；$\beta$为质心侧偏角，指车辆速度方向和车身朝向两者间所成的角度；$\Psi$为航向角（Heading Angle），指车身与大地坐标系X轴的夹角。
基于自行车模型（车辆运动学模型），由正弦定理可知：
$$\frac{sin({\delta }_f-\beta )}{l_f}=\frac{sin(\frac{\pi }{2}-{\delta }_f)}{R}\text{\hspace{1em}(1.1)}$$

$$\frac{sin(\beta -{\delta }_r)}{l_r}=\frac{sin(\frac{\pi }{2}+{\delta }_r)}{R}\text{\hspace{1em}(1.2)}$$
展开上述公式(1)(2)两个公式可得：
$$\frac{sin{\delta }_{f}cos\beta -sin\beta cos{\delta }_f}{l_f}=\frac{sin{\delta }_f}{R}\text{\hspace{1em}(1.3)}$$

$$\frac{cos{\delta }_{r}sin\beta -cos\beta sin{\delta }_r}{l_r}=\frac{cos{\delta }_r}{R}\text{\hspace{1em}(1.4)}$$

联立上述(3)(4)两个公式可得：

$$(tan{\delta }_f-tan{\delta }_r)cos\beta =\frac{l_f+l_r}{R}\text{\hspace{1em}(1.5)}$$

低速环境下，车辆行驶路径的转弯半径变化缓慢，此时我们可以假设车辆的航向角变化率等于车辆的横摆角速度。则车辆的航向角变化率(横摆角速度)为：

$$\dot{\Psi }=\frac{V}{R}\text{\hspace{1em}(1.6)}$$

联立公式(5)(6)可得：

$$\dot{\Psi }=\frac{Vcos\beta }{l_f+l_r}(tan{\delta }_f-tan{\delta }_r)\text{\hspace{1em}(1.7)}$$

因此，在惯性坐标系下，可得车辆运动学模型：

$$\{\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)\end{array}\text{\hspace{1em}(1.8)}$$

此模型中有三个输入：${\delta }_f$、${\delta }_r$、V.其中$\beta$可由公式（3）（4）求得：

$$\beta =ta{n}^{-1}(\frac{l_{f}tan{\delta }_r+l_{r}tan{\delta }_f}{l_f+l_r})\text{\hspace{1em}(1.9)}$$

阿克曼转向几何(Ackerman turning geometry)

阿克曼转向几何(Ackerman Turning Geometry)是一种为了解决交通工具转弯时，内外转向轮路径指向的圆心不同的几何学。
在单车模型中，将转向时左/右前轮偏角假设为同一角度，虽然通常两个角度大致相等，但实际并不是，通常情况下，内侧轮胎转角更大。如下图所示，${\delta }_o$和${\delta }_i$分别为外侧前轮和内侧前轮偏角，当车辆右转时，右前轮胎为内侧轮胎，其转角${\delta }_i$较左前轮胎转角${\delta }_o$更大。其中$l_w$为轮距，L为轴距，后轮两轮胎转角始终为0°.当以后轴中心为参考点时，转向半径R为下图中$红线$。

当质心侧偏角$\beta$很小，且后轮转向角为0时，公式（1.7）可表述为：

$$\dot{\Psi }=\frac{1}{R}\approx \frac{V}{L}tan{\delta }_f\approx \frac{V}{L}{\delta }_f\text{\hspace{1em}(1.10)}$$

由于内外侧轮胎的转向半径不同，因此有：

$${\delta }_o=\frac{L}{R+\frac{l_w}{2}}$$

$${\delta }_i=\frac{L}{R-\frac{l_w}{2}}\text{\hspace{1em}(1.11)}$$

则前轮平均转向角为：

$$\delta =\frac{{\delta }_o+{\delta }_i}{2}\approx \frac{L}{R}\text{\hspace{1em}(1.12)}$$

内外轮转角差为：

$$\Delta \delta ={\delta }_i-{\delta }_o={\delta }^2\frac{l_w}{L}\text{\hspace{1em}(1.13)}$$

因此，两个前轮的转角差与平均转向角的平方成正比。

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

车辆运动学模型小结

上述推导是以质心为参考点，但是工程中一般以后轴中心为参考点，以单车模型为基础建立车辆的运动学模型，其前轮转角为${\delta }_f$,后轮转角${\delta }_r$为0，质心侧偏角$\beta \approx 0$，车辆后轴中心的坐标为$(x,y)$,纵向车速为$v$，航向角（这里也就是横摆角）为$\Psi$，因此，车辆的状态变量可为$[x,y,\Psi {]}^T$，公式（1.8）可转换车辆运动学模型的状态空间方程形式:

$$\dot{X}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\Psi }\end{array}\right]=\left[\begin{array}{c}cos\Psi \\ \sin \Psi \\ \frac{tan{\delta }_f}{L}\end{array}\right]v\text{\hspace{1em}(1.14)}$$

基于车辆运动学建立道路-车辆误差模型(线性化)

由车辆运动模型可知，车辆状态变量为$X=[x,y,\Psi {]}^T$，控制变量取$U=[v,\delta {]}^T$，则有车辆状态空间方程（此时为非线性状态空间方程）为：

$$\dot{X}=f(X,U)=\left[\begin{array}{c}cos\Psi \\ sin\Psi \\ \frac{tan\delta }{L}\end{array}\right]v\text{\hspace{1em}(1.15)}$$

假设参考轨迹点的车辆参考状态和参考控制量为$(X_{ref},U_{ref})$，则车辆在参考点的状态空间方程为：

$$\dot{X_{ref}}=f(X_{ref},U_{ref})=\left[\begin{array}{c}cos{\Psi }_{ref}\\ sin{\Psi }_{ref}\\ \frac{tan{\delta }_{ref}}{L}\end{array}\right]v_{ref}\text{\hspace{1em}(1.16)}$$

对上式在参考点$(X_{ref},U_{ref})$采用一阶泰勒展开，得到：

$$\dot{X}=\dot{X_{ref}}+\frac{df(X,U)}{dX}{|}_{(X_{ref},U_{ref})}(X-X_r)+\frac{df(X,U)}{dU}{|}_{(X_{ref},U_{ref})}(U-U_{ref})\text{\hspace{1em}(1.17)}$$

其中：

$$\frac{df(X,U)}{dX}{|}_{(X_{ref},U_{ref})}=\left[\begin{array}{ccc}\frac{d{f}_1}{dx}& \frac{d{f}_1}{dy}& \frac{d{f}_1}{d\Psi }\\ \frac{d{f}_2}{dx}& \frac{d{f}_2}{dy}& \frac{d{f}_2}{d\Psi }\\ \frac{d{f}_3}{dx}& \frac{d{f}_3}{dy}& \frac{d{f}_3}{d\Psi }\end{array}\right]{|}_{(X_{ref},U_{ref})}=\left[\begin{array}{ccc}0& 0& -v_{ref}\ast sin{\Psi }_{ref}\\ 0& 0& v_{ref}\ast cos{\Psi }_{ref}\\ 0& 0& 0\end{array}\right]\text{\hspace{1em}(1.18)}$$

$$\frac{df(X,U)}{dU}{|}_{(X_{ref},U_{ref})}=\left[\begin{array}{cc}\frac{d{f}_1}{dv}& \frac{d{f}_1}{d\delta }\\ \frac{d{f}_2}{dv}& \frac{d{f}_2}{d\delta }\\ \frac{d{f}_3}{dv}& \frac{d{f}_3}{d\delta }\end{array}\right]{|}_{(X_{ref},U_{ref})}=\left[\begin{array}{cc}cos{\Psi }_{ref}& 0\\ sin{\Psi }_{ref}& 0\\ \frac{tan{\Psi }_{ref}}{L}& \frac{v_{ref}}{L\ast co{s}^2{\delta }_{ref}}\end{array}\right]\text{\hspace{1em}(1.19)}$$

根据上述公式，可知：

$$\dot{X}-\dot{X_{ref}}=\left[\begin{array}{ccc}0& 0& -v_{ref}\ast sin{\Psi }_{ref}\\ 0& 0& v_{ref}\ast cos{\Psi }_{ref}\\ 0& 0& 0\end{array}\right](X-X_r)+\left[\begin{array}{cc}cos{\Psi }_{ref}& 0\\ sin{\Psi }_{ref}& 0\\ \frac{tan{\Psi }_{ref}}{L}& \frac{v_{ref}}{L\ast co{s}^2{\delta }_{ref}}\end{array}\right](U-U_{ref})\text{\hspace{1em}(1.20)}$$

因此，令误差状态变量为$\tilde{X}$，基于运动学的道路-车辆误差状态方程可表示为：

$$\dot{\tilde{X}}=A\tilde{X}+B\tilde{U}\text{\hspace{1em}(1.21)}$$

其中:

$$\begin{gathered} \dot{\tilde{X}}=\left[\begin{array}{c}\dot{x}-\dot{x_{ref}}\\ \dot{y}-\dot{y_{ref}}\\ \dot{\Psi }-\dot{{\Psi }_{ref}}\end{array}\right]，\tilde{X}=\left[\begin{array}{c}x-x_{ref}\\ y-y_{ref}\\ \Psi -{\Psi }_{ref}\end{array}\right], \\ A=\left[\begin{array}{ccc}0& 0& -v_{ref}\ast sin{\Psi }_{ref}\\ 0& 0& v_{ref}\ast cos{\Psi }_{ref}\\ 0& 0& 0\end{array}\right]B=\left[\begin{array}{cc}cos{\Psi }_{ref}& 0\\ sin{\Psi }_{ref}& 0\\ \frac{tan{\Psi }_{ref}}{L}& \frac{v_{ref}}{L\ast co{s}^2{\delta }_{ref}}\end{array}\right],\tilde{U}=\left[\begin{array}{c}v-v_{ref}\\ \delta -{\delta }_{ref}\end{array}\right]\text{\hspace{1em}(1.22)} \end{gathered}$$

其中，参考轨迹点的参考前轮转角${\delta }_{ref}$可通过前阿克曼转向原理确定，即$\tan {\delta }_{ref}=\frac{L}{R}或L\ast kappa$.
基于此误差模型计算出来的控制量$\tilde{U}$为变化量，最终的控制量应该为：$U=\tilde{U}+U_{ref}$

基于车辆运动学的道路-车辆误差模型的离散化

前向欧拉：

$$\begin{gathered} \frac{{\tilde{X}}_{k+1}-{\tilde{X}}_k}{T}=\dot{{\tilde{X}}_k}=A{\tilde{X}}_k+B{U}_k \\ {\tilde{X}}_{k+1}=(A\ast T+I){\tilde{X}}_k+B\ast T{U}_k\text{\hspace{1em}(1.23)} \end{gathered}$$

那么，离散误差状态方程：

$${\tilde{X}}_{k+1}=A_d{\tilde{X}}_k+B_d{U}_k\text{\hspace{1em}(1.24)}$$

其中：$A_d=(A\ast T+I),B_d=B\ast T$