自动驾驶中的车辆运动学模型

半杯茶的小酒杯

要控制车辆的运动，首先要对车辆的运动建立数字化模型，模型建立的越准确，对车辆运动的描述越准确，对车辆的跟踪控制的效果就越好。除了真实反映车辆特性外，建立的模型也应该尽可能的简单易用。自行车模型(Bicycle Model)是一种常见的车辆运动学模型。

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

1）不考虑车辆在垂直方向(Z轴方向)的运动，即假设车辆的运动是一个二维平面上的运动。

2) 假设车辆左右侧轮胎在任意时刻都拥有相同的转向角度和转速；这样车辆的左右两个轮胎的运动可以合并为一个轮胎来描述。

3）假设车辆行驶速度变化缓慢，忽略前后轴载荷的转移。

4) 假设车身和悬架系统都是 刚性系统 。

5）假设车辆的运动和转向是由前轮驱动( front − wheel − only ) 的。

1、以后轴为原点的车辆运动模型

自动驾驶中的车辆模型可以简化为二维平面上运动的刚体结构，任意时刻车辆的状态 q=(x,y, \theta) ，车辆坐标的原点位于后轴的中心位置，坐标轴与车身平行。s表示车辆的速度， \phi 表示steering angle(左为正，右为负)，L表示前轮和后轮的距离，如果steering angle保持 \phi 不变，车辆就会在原地转圈，半径为 \rho 。

车辆运动模型:

\dot{x} = f_1 (x, y, \theta, s, \phi)

\dot{y} = f_2 (x, y, \theta, s, \phi)

\dot{\theta} = f_3(x,y, \theta, s, \phi)

在一个非常短的时间 \Delta t 内，可以近似认为车辆按照车身的方向运动， d_x 、 d_y 表示在 d_t 时间内车辆在x轴、y轴移动的距离。

由

\frac{dy}{dx} = tan{\theta}

\frac{dy}{dx} = \frac{\dot{y}}{\dot{x}}

tan{\theta} = sin{\theta} / cos{\theta}

可以推出:

-\dot{x} sin{\theta} + \dot{y} cos{\theta} = 0

用 \omega 表示 \Delta t 时间内车身运动的距离，于是由

d \omega = \rho d {\theta}

\rho = L / tan {\phi}

可以推出:

d \theta = \frac{tan{\phi}}{L} d \omega

对等式两侧除以 d_t ，并且基于

\dot{\omega} = s

的条件可以最终得到等式:

\dot{\theta} = \frac{s}{L} tan{\phi}

至此一个简化的非完整约束的车辆运动模型完成了，汇总如下:

\begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{bmatrix} = s * \begin{bmatrix} cos(\theta) \\ sin(\theta) \\ \frac{tan \phi}{L} \end{bmatrix}

\dot{s} = a

基于这个简单的运动学模型，在给定了某个时刻的控制输入(a, \phi )以后，我们就可以估算车辆在下一时刻的状态信息(坐标，偏航角以及速度)。

2、以质心为中心的车辆运动学模型

其中A点是前轮，B是后轮，C为车辆质心点，O为OA、OB的交点，是车辆的瞬时滚动中心，线段OA、OB分别垂直于两个滚动轮的方向； \beta 为滑移角(Tire Slip Angle)，指车辆行进方向和轮圈所指方向两者间所成的角度； \psi 为航向角(Heading Angle)，指车身与X轴的夹角。

\frac{\sin(\delta_f-\beta)}{\ell_f}=\frac{\sin(\frac{\pi}{2}-\delta_f)}{R} \tag{1.1}

\frac{\sin(\beta-\delta_r)}{\ell_r}=\frac{\sin(\frac{\pi}{2}+\delta_r)}{R} \tag{1.2}

展开公式(1.1)(1.2)可得：

\frac{\sin\delta_f\cos\beta-\sin\beta\cos\delta_f}{\ell_f}=\frac{\cos\delta_f}{R} \tag{1.3}

\frac{\cos\delta_r\sin\beta-\cos\beta\sin\delta_r}{\ell_r}=\frac{\cos\delta_r}{R} \tag{1.4}

联立公式(1.3)(1.4)可得：

(\tan\delta_f-\tan\delta_r)\cos\beta=\frac{\ell_f+\ell_r}{R} \tag{1.5}

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

\dot{\psi}=\frac{V}{R} \tag{1.6}

联立公式(1.5)(1.6)可得：

\dot{\psi}=\frac{V\cos\beta}{\ell_f+\ell_r}(\tan\delta_f-\tan\delta_r) \tag{1.7}

则在惯性坐标系XY下，可得车辆运动学模型:

\begin{cases} \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) \tag{1.8} \end{cases}

此模型中有三个输入： \delta_f 、 \delta_r 、和V。

滑移角 \beta 可由公式(1.3)(1.4)求得：

\beta=\tan^{-1}(\frac{\ell_f\tan\delta_r+\ell_r\tan\delta_f}{\ell_f+\ell_r}) \tag{1.9}

作为一种自行车模型，运动学自行车模型也假定车辆形如一辆自行车，整个的控制量可以简化为 (a, \delta_f) ，其中a是车辆的加速度，踩油门踏板意味着正的加速度；踩刹车踏板意味着负的加速度； \delta_f 是方向盘转角。