【自动驾驶】车辆运动学模型

参考资料

1. 自动驾驶中的车辆运动学模型
2. 车辆数学模型
3. 车辆运动学模型
4. 车辆控制-运动学模型(Kinematic Model)
5. 运动学模型及其线性化

模型的用处就是在当前状态给定某控制输入时，预测（估计） 系统未来的状态。控制领域利用模型设计合适的输入，以期控制系统到达目标状态。

1. 以车辆重心为中心的单车运动学模型

1.1 参数说明

一般考虑运动学模型时，将车辆模型简化成单车模型(bicycle model)。

单车模型中：

• 左右轮等效为单个轮子
  左右前轮合并为单个轮子，其中心点为A点，同样后轮等效后的中心点为 B点。

• 转向角
  前后轮的转向角用${\delta }_f$​和${\delta }_r$​表示，模型中前后轮都可以转向，对于只有前轮转向的系统，后轮转向角${\delta }_r$可以设置为0.

• 重心
  点 C 代表车辆的重心， A 点和 B点到重心的距离分别用 $l_f$和 $l_r$​表示，轴距表示为$L=l_f+l_r$。

• 速度
  车辆重心的速度用$V$表示，与车辆纵向轴的夹角为$\beta$，该角度叫做车辆的滑移角。

• 运动描述
  假设车辆平动，车辆运动状态可以用三个坐标量描述： $x$ 、$y$ 和 $\psi$。其中$(x,y)$代表车辆的位置，$\psi$描述的是航向角（Heading Angle），指车身与X轴的夹角。

• 条件假设
  假设速度矢量$V$的方向在点$A$点和$B$点的方向与转向角的方向相同，换句话说，在A点的速度矢量与车辆纵轴的夹角为 ${\delta }_f$，同样 $B$点的速度矢量与车辆纵轴的夹角为${\delta }_r$。也就是说前后轮的滑移角$\beta$都为0。该条件假设成立前提的是车辆速度很低(<5m/s)，此时轮胎产生的横向力很小，可以忽略。

• 轨迹半径
  点 $O$代表车辆的瞬时旋转中心，线段 $AO$与$BO$与前后两个转轮方向垂直，他们的交点即为 $O$点，线段$OC$的长度代表车辆的轨迹半径$R$。

• 航迹角
  车辆重心处的速度垂直于 $OC$,车辆速度矢量与车辆纵轴的夹角为$\beta$，车辆的航向角为 $\psi$，则航迹角为$\gamma =\psi +\beta$。

1.2 几何关系

1.2.1 偏航角$\psi$的关系

如上图所示，在三角形$OCA$中，根据正弦定理，有：
$$\frac{\sin \left({\delta }_f-\beta \right)}{l_f}=\frac{\sin \left(\frac{\pi }{2}-{\delta }_f\right)}{R}\text{\hspace{1em}(1)}$$

在三角形$OBC$中，根据正弦定理，有：
$$\frac{\sin \left(\beta -{\delta }_r\right)}{l_r}=\frac{\sin \left(\frac{\pi }{2}+{\delta }_r\right)}{R}\text{\hspace{1em}(2)}$$
展开公式(1)(2)可得:
$$\frac{\sin {\delta }_f\cos \beta -\sin \beta \cos {\delta }_f}{l_f}=\frac{\cos {\delta }_f}{R}\text{\hspace{1em}(3)}$$
$$\frac{\sin \beta \cos {\delta }_r-\cos \beta \sin {\delta }_r}{l_r}=\frac{\cos {\delta }_r}{R}\text{\hspace{1em}(4)}$$

等式(3)两边同时乘 $\frac{l_f}{\cos \left({\delta }_f\right)}$ 得
$$\tan \left({\delta }_f\right)\cos (\beta )-\sin (\beta )=\frac{l_f}{R}\text{\hspace{1em}(5)}$$
同理，等式(4)两边同时乘 $\frac{l_r}{\cos \left({\delta }_r\right)}$ 得
$$\sin (\beta )-\tan \left({\delta }_r\right)\cos (\beta )=\frac{l_r}{R}\text{\hspace{1em}(6)}$$

联立公式(5)(6)可得：
$$\left(\tan {\delta }_f-\tan {\delta }_r\right)\cos \beta =\frac{l_f+l_r}{R}\text{\hspace{1em}(7)}$$

根据条件假设，低速环境下，车辆行驶路径的转弯半径变化缓慢，此时我们可以假设车辆偏航角的变化率率$\dot{\psi }$可近似等于车辆的角速度$\omega$。根据车辆角速度$\omega =\frac{V}{R}$得
$$\dot{\psi }=\frac{V}{R}\text{\hspace{1em}(8)}$$
将公式(8)带入公式(7)中，消除 $R$项得
$$\dot{\psi }=\frac{V\cos \beta }{l_f+l_r}\left(\tan {\delta }_f-\tan {\delta }_r\right)\text{\hspace{1em}(9)}$$

1.2.1 滑移角 $\beta$ 的关系

等式(5)乘以 $l_r$ 得
$$\tan \left({\delta }_f\right)\cos (\beta )l_r-\sin (\beta )l_r=\frac{l_f\cdot l_r}{R}\text{\hspace{1em}(10)}$$
等式(6)乘以 $l_f$ 得
$$\sin (\beta )l_f-\tan \left({\delta }_r\right)\cos (\beta )l_f=\frac{l_f\cdot l_r}{R}\text{\hspace{1em}(11)}$$
等式(10)和(11)相减得
$$\cos (\beta )\left(l_f\tan \left({\delta }_r\right)+l_r\tan \left({\delta }_f\right)\right)=\sin (\beta )\left(l_f+l_r\right)\text{\hspace{1em}(12)}$$
等式(12)两端同时乘以 $\frac{1}{\cos (\beta )}$ 得
$$\tan (\beta )=\frac{l_f\tan \left({\delta }_r\right)+l_r\tan \left({\delta }_f\right)}{l_f+l_r}\text{\hspace{1em}(13)}$$
故取反三角函数得
$$\beta =\arctan \left(\frac{l_f\tan \left({\delta }_r\right)+l_r\tan \left({\delta }_f\right)}{l_f+l_r}\right)\text{\hspace{1em}(14)}$$

1.2.2 运动学模型

根据上图，很容易得到$x,y$方向的速度为
$$\begin{array}{rl}& \dot{x}=V\cos (\beta +\psi )\\ & \dot{y}=V\sin (\beta +\psi )\end{array}\text{\hspace{1em}(15)}$$

综上，以车辆重心为中心的运动学模型为

$$\{\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}\left(\tan {\delta }_f-\tan {\delta }_r\right)\end{array}\text{\hspace{1em}(16)}$$
其中，
$$\beta =\arctan \left(\frac{l_f\tan \left({\delta }_r\right)+l_r\tan \left({\delta }_f\right)}{l_f+l_r}\right)$$

1.3 python 实现

class KinematicModel_1:
  """假设控制量为前后轮的转向角delta_f，delta_r和加速度a
  """

  def __init__(self, x, y, psi, v, l_r, l_f, dt):
    self.x = x
    self.y = y
    self.psi = psi
    self.v = v
    self.l_f = l_f
    self.l_r = l_r
    # 实现是离散的模型
    self.dt = dt

  def update_state(self, a, delta_f,delta_r):
    beta = math.atan((self.l_r*math.tan(delta_f)+self.l_f*math.tan(delta_r))/(self.l_f+self.l_r))
    self.x = self.x+self.v*math.cos(self.psi+beta)*self.dt
    self.y = self.y+self.v*math.sin(self.psi+beta)*self.dt
    self.psi = self.psi+self.v*math.cos(beta)*(math.tan(delta_f)-math.tan(delta_r))/(self.l_f+self.l_r)*self.dt
    self.v = self.v+a*self.dt

  def get_state(self):
    return self.x, self.y, self.psi, self.v

2. 以前轮驱动的单车运动学模型

2.1 几何关系

由于绝大多数的汽车后轮都不能够偏转，所以在单车模型基础上，我们假定后轮的转角控制输入${\delta }_r=0$，即车辆为前轮驱动(front−wheel−only)。也就是说，方向盘上的控制输入，都反映到了前轮的转角上了，即认为方向盘的转角就等于前轮的转角${\delta }_f$。

注意：这里依旧以车辆重心为中心。

在直角三角形$OBC$中，易得
$$\sin \beta =\frac{l_r}{R}\text{\hspace{1em}(17)}$$

将公式(8)代入公式(17)得

$$\dot{\psi }=\frac{V\sin \beta }{l_r}\text{\hspace{1em}(18)}$$

故前轮驱动的车辆运动学模型为
$$\{\begin{array}{l}\dot{x}=V\cos (\psi +\beta )\\ \dot{y}=V\sin (\psi +\beta )\\ \dot{\psi }=\frac{V\sin \beta }{l_r}\end{array}\text{\hspace{1em}(19)}$$
其中，$\beta$的推导方式与前文一致（可直接令公式(14)的${\delta }_r=0$），可得
$$\beta =\arctan \left(\frac{l_r}{l_f+l_r}\tan \left({\delta }_f\right)\right)\text{\hspace{1em}(20)}$$

2.2 python实现

class KinematicModel_2:
  """假设控制量为前轮的转向角delta_f和加速度a
  """
  def __init__(self, x, y, psi,v,l_r,l_f,dt):
    self.x = x
    self.y = y
    self.psi = psi
    self.v = v
    self.l_f = l_f
    self.l_r = l_r
    # 实现是离散的模型
    self.dt=dt
  

  def update_state(self,a,delta_f):
    beta = math.atan((self.l_r)/(self.l_f+self.l_r)*math.tan(delta_f))
    self.x = self.x+self.v*math.cos(self.psi+beta)*self.dt
    self.y = self.y+self.v*math.sin(self.psi+beta)*self.dt
    self.psi = self.psi+self.v*math.sin(beta)/self.l_r*self.dt
    self.v = self.v+a*self.dt

  def get_state(self):
    return self.x, self.y, self.psi, self.v
      

3. 以后轴中心为车辆中心的单车运动学模型

3.1 几何关系

在直角三角形$OBA$中，显然有
$$\tan {\delta }_f=\frac{L}{R}\text{\hspace{1em}(21)}$$

联立公式(8)，可得：
$$\dot{\psi }=\frac{V}{L}\tan {\delta }_f$$

另外，根据几何关系，显然有

$$\begin{gathered} \dot{x}=V\cos (\psi ) \\ \dot{y}=V\sin (\psi ) \end{gathered}$$

因此，以后轴中心为车辆中心的运动学模型为

$$\{\begin{array}{l}\dot{x}=V\cos (\psi )\\ \dot{y}=V\sin (\psi )\\ \dot{\psi }=\frac{V}{L}\tan {\delta }_f\end{array}\text{\hspace{1em}(22)}$$

如果使用车辆的加速度$a$作为 控制，则再加上下面这个公式即可
$$\dot{V}=a\text{\hspace{1em}(23)}$$

但在无人车控制过程中，一般控制对象 $u={\left[v,w\right]}^T$ ，则式(22)可写为:
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{c}\cos \psi \\ \sin \psi \\ 0\end{array}\right]v+\left[\begin{array}{l}0\\ 0\\ 1\end{array}\right]w$$

速度 $v$ 的控制主要通过刹车 (brake) 、油门 (throttle) 、档位 (gear) 等来控制，横摆角速度 $w$主要通过转动方向盘 (steer) 来 控制。

3.2 python实现

class KinematicModel_3:
  """假设控制量为转向角delta_f和加速度a
  """
  def __init__(self, x, y, psi,v,L,dt):
    self.x = x
    self.y = y
    self.psi = psi
    self.v = v
    self.L = L
    # 实现是离散的模型
    self.dt=dt
  

  def update_state(self,a,delta_f):
    self.x = self.x+self.v*math.cos(self.psi)*self.dt
    self.y = self.y+self.v*math.sin(self.psi)*self.dt
    self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt
    self.v = self.v+a*self.dt

  def get_state(self):
    return self.x, self.y, self.psi, self.v
      

所有实现代码欢迎访问我的github仓库，正在持续更新中~~

4. 阿克曼转向几何

汽车采用阿克曼转向轮。阿克曼转向几何(Ackerman Turning Geometry)是一种为了解决交通工具转弯时，内外转向轮路径指向的圆心不同的几何学。

在单车模型中，将转向时左、右前轮偏角假设为同一角度${\delta }_f$，虽然通常两个角度大致相等，但实际并不是，通常情况下，内侧轮胎转角更大。如下图所示。

${\delta }_o$ 和 ${\delta }_i$ 分别为外侧前轮和内侧前轮偏角，当车辆左转时，左前轮胎为内侧轮胎，其转角 ${\delta }_i$ 较 右前轮胎转角 ${\delta }_o$ 更大。 $l_w$ 为轮距， $L=l_f+l_r$ 为轴距，远远小于轨迹半径$R$，滑移角$\beta$接近于0。一般车辆模型后轴为固定轴，故后轮两轮胎转角为 $0^{\circ }$，即${\delta }_r$为0 。

当以后轴中心为参考点时，则$OB$为转向半径R。

当滑移角$\beta$很小时，且后轮偏角为0时，公式(9)可近似为

$$\dot{\psi }\approx \frac{V}{L}\tan \left({\delta }_f\right)\text{\hspace{1em}(24)}$$
由于 ${\delta }_f$ 很小
$$\tan \left({\delta }_f\right)\approx {\delta }_f\text{\hspace{1em}(25)}$$
根据公式(8)和公式(24)得
$$\frac{\dot{\psi }}{V}\approx \frac{{\delta }_f}{L}=\frac{1}{R}\text{\hspace{1em}(26)}$$
故不区分前后轴，等效转向角为
$$\delta =\frac{L}{R}\text{\hspace{1em}(27)}$$
由于内外轮的转弯半径不同，根据公式(27), 外轮转角为
$${\delta }_o=\frac{L}{R+\frac{l_w}{2}}\text{\hspace{1em}(28)}$$

内轮转角为
$${\delta }_i=\frac{L}{R-\frac{l_w}{2}}\text{\hspace{1em}(29)}$$
故前轮平均转向角为
$$\delta =\frac{{\delta }_o+{\delta }_i}{2}=\frac{L}{R-\frac{l_w^2}{4R}}\text{\hspace{1em}(30)}$$
由于 $\frac{l_w^2}{4R}$ 项中， $l_w$ 远远小于 $R$, 且 $l_w$ 的二次项更小，故
$$\frac{l_w^2}{4R}\cong 0\text{\hspace{1em}(31)}$$
所以等式(30)可以近似为
$$\delta =\frac{L}{R}\text{\hspace{1em}(32)}$$
比较等式(28)和(29)知， ${\delta }_i$ 始终大于 ${\delta }_o$ ，故
$$\begin{array}{rl}{\delta }_i-{\delta }_o& =\frac{L}{R-\frac{l_w}{2}}-\frac{L}{R+\frac{l_w}{2}}\\ & =\frac{L{l}_w}{R^2-\frac{l_w^2}{4}}\\ & \cong \frac{L}{R^2}{l}_w={\delta }^2\frac{l_w}{L}\end{array}\text{\hspace{1em}(33)}$$
根据公式(33)可知，前轮内外转向角的差值接近于平均转向角的二次方，所以当前轮转向角较大时， 内外轮的转向角误差就越大。

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

车辆运动模型基于单车模型推导，推导过程不考虑车辆受到的横向力，故该模型只适用于车辆速度很低的情形。
$$a=\frac{m{V}^2}{R}\text{\hspace{1em}(34)}$$
根据公式(34)知，速度很小时，车辆受到的向心力可以忽略不记，所以才有公式(8)的成立。所以当车辆的运动场景速度较低时，可以使用该模型描述车辆的运动。