自动驾驶规划控制系列文章（一）—

规划控制模块在整个自动驾驶领域占有相当重要的位置；对规划而言，能够规划出一条符合车辆运动规律的轨迹对底层的控制来说是至关重要。对于控制而言，其建立的模型越准确，则控制通常就会更加精准。因此建立比较精确的车辆数学模型对规划和控制都相当重要。本文以将车辆简化为自行车模型，然后分别建立运动学模型和动力学模型。

一、车辆自行车运动学模型（kinematic bicycle model）

运动学模型基于如下假设：

不考虑车辆在垂直方向（z轴方向上的运动），即假设车辆在一个二维平面上运动。
忽略车辆受力情况，只考虑车辆在运动中的几何关系；
两个前轮（后轮）合并成一个单独的轮胎（自行车模型的由来）；
轮胎运动方向和轮胎平面水平.

1.基于车辆重心的运动学模型

车辆自行车模型示意如下图所示。

点O为车辆旋转中心，A为前轮中心，B为后轮中心，C为重心（质心）。
其它符号含义入下：

符号	含义	符号	含义
δf\delta_fδf	前轮偏角	δr\delta_rδr	后轮偏角
lrl_r lr	后轮中心到重心之间的距离	lfl_flf	前轮中心到重心的距离
vvv	重心速度	β\betaβ	滑移角
ψ\psiψ	航向角	u1u_1u1	车辆加速度
xxx	车辆x轴方向位置	yyy	y轴位置

其根据几何关系可以得到如下数学模型¹ ²
根据三角形正弦定理，对于OCA有：
sin(δf−β)lf=sin(π2−δf)R(1.1)\frac{sin(\delta_f-\beta)}{l_f} = \frac{sin(\frac{\pi}{2}-\delta_f)}{R}\:\:\:\:\:\:\:\:\:\:\:\:\:\:\:\:\:\:(1.1) lfsin(δf−β)=Rsin(2π−δf)(1.1)
对OCB有：
sin(β−δr)lr=sin(π2+δr)R(1.2)\frac{sin(\beta-\delta_r)}{l_r} = \frac{sin(\frac{\pi}{2}+\delta_r)}{R}\:\:\:\:\:\:\:\:\:\:\:\:\:\:\:\:\:\:(1.2) lrsin(β−δr)=Rsin(2π+δr)(1.2)
从（1.1）得到：
sin(δf)cos(β)−sin(β)cos(δf)lf=δfR(1.3)\frac{sin(\delta_f)cos(\beta) - sin(\beta)cos(\delta_f)}{l_f} = \frac{\delta_f}{R}\:\:\:\:\:\:\:\:\:\:\:\:\:(1.3) lfsin(δf)cos(β)−sin(β)cos(δf)=Rδf(1.3)
从（1.2）得到：
cos(δr)sin(β)−cos(β)sin(δr)lr=cos(δr)R(1.4)\frac{cos(\delta_r)sin(\beta) - cos(\beta)sin(\delta_r)}{l_r} = \frac{cos(\delta_r)}{R}\:\:\:\:\:\:\:\:\:\:\:\:\:(1.4) lrcos(δr)sin(β)−cos(β)sin(δr)=Rcos(δr)(1.4)
(3)两边乘以lfcos(δf)\frac{l_f}{cos(\delta_f)}cos(δf)lf得到：
tan(δf)cos(β)−sin(β)=lfR(1.5)tan(\delta_f)cos(\beta)-sin(\beta)=\frac{l_f}{R}\:\:\:\:\:\:\:\:\:\:\:\:\:(1.5) tan(δf)cos(β)−sin(β)=Rlf(1.5)
同理（4）两边乘以lrcos(δr)\frac{l_r}{cos(\delta_r)}cos(δr)lr得到：
sin(β)−tan(δr)cos(β)=lrR(1.6)sin(\beta)-tan(\delta_r)cos(\beta) = \frac{l_r}{R}\:\:\:\:\:\:\:\:\:\:\:\:\:(1.6) sin(β)−tan(δr)cos(β)=Rlr(1.6)
将(5)(6)相加得：
(tan(δf)−tan(δr))cos(β)=lf+lrR(1.7)(tan(\delta_f)-tan(\delta_r))cos(\beta)=\frac{l_f+l_r}{R}\:\:\:\:\:\:\:\:\:\:\:\:\:(1.7) (tan(δf)−tan(δr))cos(β)=Rlf+lr(1.7)
在上述假设条件下，很容易得到如下关系：
ψ˙=vR(1.8)\dot{\psi} = \frac{v}{R}\:\:\:\:\:\:\:\:\:\:\:\:\:(1.8) ψ˙=Rv(1.8)
代入(1.7)中，可以得到：
ψ˙=vcos(β)lf+lr(tan(δf)−tan(δr))(1.9)\dot{\psi} = \frac{vcos(\beta)}{l_f+l_r}(tan(\delta_f)-tan(\delta_r))\:\:\:\:\:\:\:\:\:\:\:\:\:(1.9) ψ˙=lf+lrvcos(β)(tan(δf)−tan(δr))(1.9)
综上，可得到自行车运动学模型：
x˙=vcos(ψ+β)y˙=vsin(ψ+β)ψ˙=vcos(β)lr+lftan(δf−δr)v˙=u1}(1.10)\left. \begin{matrix} \dot{x} = v\,cos(\psi + \beta) \\ \dot{y} = v\,sin(\psi + \beta) \\ \dot{\psi} = \frac{v cos(\beta)}{l_r + l_f}\, tan(\delta_f-\delta_r) \\ \dot{v} = u_1\\ \end{matrix} \right\}\:\:\:\:\:\:\:\:\:\:\:\:\:(1.10) x˙=vcos(ψ+β)y˙=vsin(ψ+β)ψ˙=lr+lfvcos(β)tan(δf−δr)v˙=u1⎭⎪⎪⎬⎪⎪⎫(1.10)

其中
β=atan[lrtan(δf)+lftan(δr)lr+lf](1.11)\beta = atan[\frac{l_rtan(\delta_f)+l_ftan(\delta_r)}{l_r+l_f}] \:\:\:\:\:\:\:\:\:\:\:\:\:(1.11) β=atan[lr+lflrtan(δf)+lftan(δr)](1.11)

2. 以后轴中心为原点的运动学模型

根据几何关系可以的得到如下模型
{x˙=s∗cos(θ)y˙=s∗sin(θ)θ˙=s∗tan(α)Ls˙=u1\begin{cases} \dot{x} = s*cos(\theta) \\ \dot{y} = s*sin(\theta) \\ \dot{\theta} = s* \frac{tan(\alpha)}{L} \\ \dot{s} = u_1 \end{cases} ⎩⎪⎪⎪⎨⎪⎪⎪⎧x˙=s∗cos(θ)y˙=s∗sin(θ)θ˙=s∗Ltan(α)s˙=u1
其中s为车速（后轴中心速度），u1 为加速度，基于该模型，当我们确定了某个控制输入(u1,α)(u_1,\alpha)(u1,α)时，我们就可以估算下一时刻车辆的状态信息。

二、车辆自行车动力学模型（kinematic bicycle model）

上面介绍的运动学模型在速度较低时，能够较为准确的反映车辆运动状态，但在高速条件下，其车轮的侧滑角已经不能在忽略。下面将建立自行车的动力学模型。

1、基本模型建立

在建模之前我们作如下假设：

车辆所受的空气的力只会对车身坐标系x轴方向上的运动有影响，y轴方向和沿着z轴的旋转不会受到空气力的影响；
车辆运行在二维平面中，也就是z轴无速度。
车辆轮胎力运行在线性区间。

建立如下坐标系，X,Y表示全局坐标系，x,y则表示车身坐标系，x轴方向沿车辆中轴方向向前，y轴方向朝右，其原点在质心位置。

根据牛顿运动定律有：
may=Fyf+Fyr(2.1)ma_y = F_{yf} + F_{yr}\qquad\qquad(2.1) may=Fyf+Fyr(2.1)
其中ay=d2ydt2a_y=\frac{d^2y}{dt^2}ay=dt2d2y表示在y方向的质心加速度。
max=Fxf+Fxr−Faero(2.2)m a_x = F_{xf} + F_ {xr}-F_{aero}\qquad\qquad(2.2) max=Fxf+Fxr−Faero(2.2)
其中：ax=d2xdt2a_x=\frac{d^2x}{dt^2}ax=dt2d2x表示在x方向的质心加速度。

Fyf\qquad F_{yf}Fyf、 FyrF_{yr}Fyr分别表示车前轮和后轮所受到的力在y轴方向上的分量。

Fxf\qquad F_{xf}Fxf、 FxrF_{xr}Fxr分别表示车前轮和后轮所受到的力在x轴方向上的分量。

Faero\qquad F_{aero}Faero、 FxrF_{xr}Fxr表示车在x轴方向受到的空气阻力。

其沿着x和y轴方向上的加速度包括两部分，一部分是沿着轴方向运行加速度，另一部分为离心加速度。
ax=vx˙−vyψ˙ay=vy˙+vxψ˙}(2.3)\left. \begin{matrix} a_x = \dot{v_x}-v_y\dot{\psi} \\ a_y = \dot{v_y}+v_x \dot{\psi} \end{matrix} \right\}\qquad\qquad(2.3) ax=vx˙−vyψ˙ay=vy˙+vxψ˙}(2.3)
另外，在z轴方向转动方程：
Izψ¨=lfFyf−lrFyr(2.4)I_z \ddot{\psi} = l_f F_{yf} - l _r F_{yr}\qquad\qquad(2.4) Izψ¨=lfFyf−lrFyr(2.4)

2、力的计算

车辆轮胎在y轴方向受到的力FyfF_{yf}Fyf、 FyrF_{yr}Fyr实验结果表明，其大小正比于轮胎的侧滑角。其侧滑角如下图所示：

可以得到，前轮侧滑角：
αf=δ−θvf(2.5)\alpha_f = \delta -\theta_{vf}\qquad\qquad(2.5)αf=δ−θvf(2.5)
后轮侧滑角：
αr=−θvr(2.6)\alpha_r= -\theta_{vr}\qquad\qquad(2.6)αr=−θvr(2.6)
因此，有：
Fyf=2Cαf(δ−θvf)(2.7)F_{yf} = 2 C_{\alpha f}(\delta-\theta_{vf})\qquad\qquad(2.7)Fyf=2Cαf(δ−θvf)(2.7)
Fyr=2Cαr(−θvr)(2.8)F_{yr} = 2 C_{\alpha r}(-\theta_{vr})\qquad\qquad(2.8)Fyr=2Cαr(−θvr)(2.8)
其中CαfC_{\alpha f}Cαf、CαrC_{\alpha r}Cαr分别表示前后轮的转弯刚度(cornering stiffness)

根据运动关系可以得到：
tan(θvf)=vy+lfψ˙vx(2.9)tan(\theta_{vf}) = \frac{v_y + l_f \dot{\psi}}{v_x}\qquad\qquad(2.9)tan(θvf)=vxvy+lfψ˙(2.9)
tan(θvr)=vy−lrψ˙vx(2.10)tan(\theta_{vr}) = \frac{v_y - l_r \dot{\psi}}{v_x}\qquad\qquad(2.10)tan(θvr)=vxvy−lrψ˙(2.10)

车辆在x轴方向的力FxfF_{xf}Fxf、 FxrF_{xr}Fxr与轮胎的滑比σx\sigma_xσx成正比。其定义为：
σx=reffωw−vxvx刹车时σx=reffωw−vxreffωw加速时}(2.11)\left. \begin{matrix} \sigma_x= \frac{r_{eff} \omega_w - v_x}{v_x} 刹车时 \\ \\ \sigma_x= \frac{r_{eff} \omega_w - v_x}{r_{eff} \omega_w} 加速时 \end{matrix} \right\}\qquad\qquad(2.11) σx=vxreffωw−vx刹车时σx=reffωwreffωw−vx加速时⎭⎪⎬⎪⎫(2.11)
因此有：
Fxf=2Cσfσxf(2.12)F_{xf} = 2 C_{\sigma f} \sigma_{xf}\qquad\qquad(2.12)Fxf=2Cσfσxf(2.12)
Fxr=2Cσrσxr(2.13)F_{xr} = 2 C_{\sigma r} \sigma_{xr}\qquad\qquad(2.13)Fxr=2Cσrσxr(2.13)
其中CσrC_{\sigma r}Cσr为纵向的轮胎刚性参数（tire stiffness parameters）。

对于空气阻力²：
Faero=12ρCdAF(vx+vwind)2(2.14)F_{aero} = \frac{1}{2}\rho C_d A_F(v_x + v_{wind})^2\qquad\qquad(2.14) Faero=21ρCdAF(vx+vwind)2(2.14)
另外在全局坐标系下：
X˙=vxcos(ψ)−vysin(ψ)Y˙=vxsin(ψ)+vycos(ψ)}(2.15)\left. \begin{matrix} \dot{X} = v_x cos(\psi) - v_y sin(\psi) \\ \dot{Y} = v_x sin(\psi) + v_y cos(\psi) \end{matrix} \right\}\qquad\qquad(2.15) X˙=vxcos(ψ)−vysin(ψ)Y˙=vxsin(ψ)+vycos(ψ)}(2.15)

3、动力学模型总结

总结上述建模过程，我们可以轻松得到如下车辆自行车动力学模型：
{X˙=vxcos(ψ)−vysin(ψ)Y˙=vxsin(ψ)+vycos(ψ)x˙=vxy˙=vyvx˙=2Cσrσxr+2Cσfσxf−12ρCdAF(vx+vwind)2m+vyψ˙vy˙=2Cαf[δ−atan(vy+lfψ˙vx)]+2Cαr[−atan(vy−lrψ˙vx)]m−vxψ˙ψ¨=2lfCαf[δ−atan(vy+lfψ˙vx)]−2lrCαr[−atan(vy−lrψ˙vx)]Iz\left \{ \begin{matrix} \dot{X} = v_x cos(\psi) - v_y sin(\psi) \\ \\ \dot{Y} = v_x sin(\psi) + v_y cos(\psi) \\ \\ \dot{x}= v_x\\ \\ \dot{y} = v_y\\ \\ \dot{v_x} = \frac{2 C_{\sigma r} \sigma_{xr} + 2 C_{\sigma f} \sigma_{xf} - \frac{1}{2}\rho C_d A_F(v_x + v_{wind})^2 }{m} + v_y\dot{\psi}\\ \\ \dot{v_y} = \frac{2 C_{\alpha f}[\delta- atan(\frac{v_y + l_f \dot{\psi}}{v_x} )]+ 2 C_{\alpha r}[-atan(\frac{v_y - l_r \dot{\psi}}{v_x})]}{m}-v_x \dot{\psi}\\ \\ \ddot{\psi} =\frac{ 2l_f C_{\alpha f}[\delta- atan(\frac{v_y + l_f \dot{\psi}}{v_x} )] - 2 l _r C_{\alpha r}[-atan(\frac{v_y - l_r \dot{\psi}}{v_x})]}{I_z} \end{matrix} \right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧X˙=vxcos(ψ)−vysin(ψ)Y˙=vxsin(ψ)+vycos(ψ)x˙=vxy˙=vyvx˙=m2Cσrσxr+2Cσfσxf−21ρCdAF(vx+vwind)2+vyψ˙vy˙=m2Cαf[δ−atan(vxvy+lfψ˙)]+2Cαr[−atan(vxvy−lrψ˙)]−vxψ˙ψ¨=Iz2lfCαf[δ−atan(vxvy+lfψ˙)]−2lrCαr[−atan(vxvy−lrψ˙)]

自动驾驶中的车辆运动学模型 ↩︎
Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006. ↩︎ ↩︎