自动驾驶运动规划 | 基于自行车模型的运动学模型和横向动力学模型详细推导图解

1 运动学 vs 动力学

机器人运动学主要研究机器人运动的几何特性而不考虑产生运动的力和力矩，它从几何角度描述机器人的位置、姿态、速度和加速度等运动关系；机器人动力学则进一步研究引起机器人运动的力和力矩之间的关系，分析控制力矩与产生的运动之间的映射规律，二者共同构成了机器人建模、轨迹规划与运动控制的理论基础。

自行车模型(Bicycle Model)是对具有前轮转向的车辆建立的简化模型，被广泛应用于现代车辆的轨迹规划和控制，此模型假设车辆仅在水平面上运动、车轮运动方向沿着车轮旋转方向(即忽略横向位移)，忽略前后轴载荷转移。本文基于自行车模型，推导车辆运动学和动力学方程。

2 车辆运动学推导

如图所示为自行车模型的几何关系，其中$A$、$B$分别表示前轮和后轮中心，$C$表示选定的车体中心，$R$是该中心对应的转向半径，$C$是瞬时转向中心(Instantaneous Center of Rotation, ICR)，其定义为车辆前后轮速度垂线的交点，是车辆在转弯时所遵循的虚拟轨迹中心——转向轴。在车辆运动学中，ICR可以用来描述车辆的转向性能以及横向稳定性，能够帮助车辆设计师和工程师更好地优化车辆运动特性。${\delta }_f$、${\delta }_r$分别是前轮和后轮的转向角；$\beta$是中心侧偏角；$\theta$是车身航向角；$l_f$、$l_r$分别是前轮和后轮距离 的距离；$v$是中心$C$的实际运动方向，$v_f$、$v_r$分别是前轮和后轮速度。

在$\Delta OBC$和$\Delta OAC$中运用正弦定理可得

$$\frac{l_f}{\sin \left({\delta }_f-\beta \right)}=\frac{R}{\sin \left(\frac{\pi }{2}-{\delta }_f\right)},\frac{l_r}{\sin \left(\beta -{\delta }_r\right)}=\frac{R}{\sin \left(\frac{\pi }{2}+{\delta }_r\right)}$$

整理可得前后轮几何方程

$$\{\begin{array}{l}\frac{l_f}{R}=\tan {\delta }_f\cos \beta -\sin \beta \\ \frac{l_r}{R}=\sin \beta -\tan {\delta }_r\cos \beta \end{array}$$

从该方程可以导出很多有用的推论：

• 中心侧偏角

  将几何方程相减可整理得到

$$\beta =\mathrm{a}\mathrm{r}\mathrm{c}\tan \left(\frac{l_r\tan {\delta }_f+l_f\tan {\delta }_r}{l_f+l_r}\right)$$

• 转向半径

  将几何方程相加可整理得到

$$R=\frac{L}{\left(\tan {\delta }_f-\tan {\delta }_r\right)\cos \beta }$$

• 运动学方程

  在全局坐标系$xOy$中车辆位姿表示为$\mathbf{p}={\left[\begin{array}{ccc}x& y& \theta \end{array}\right]}^T$，则自行车模型运动学方程为
  $$\dot{\mathbf{p}}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=v_r\left[\begin{array}{c}\cos \theta \\ \sin \theta \\ \left(\tan {\delta }_f\right)/L\end{array}\right]$$
  其中$v_r$是后轮驱动速度，那么整个模型的输入控制量为$\mathbf{u}={\left[\begin{array}{cc}{v}_r& {\delta }_f\end{array}\right]}^T$

3 车辆横向动力学推导

在自行车运动学模型中，假设轮胎具有刚体性质，即车轮运动方向沿车轮旋转方向。但在高速行驶场合，由于侧向风或行驶向心力等因素造成车轮速度与其旋转方向不一致，导致原假设失效，因此需要考虑车轮的侧偏特性

本节采用最简单的线性侧偏力假设

$$F_{\alpha }=C\alpha$$

其中$F_{\alpha }$是侧偏力，$\alpha$是侧偏角，$C$是侧偏刚度(负值)。规定$\alpha$在轮胎坐标系左侧为正。

首先以车体为惯性坐标系建模。如图所示，根据牛顿第二定律和刚体转动定律有

$$\{\begin{array}{l}m{a}_y=F_{\alpha r}\cos {\delta }_r+F_{\alpha f}\cos {\delta }_f\\ I\ddot{\theta }=F_{\alpha f}{l}_f\cos {\delta }_f-F_{\alpha r}{l}_r\cos {\delta }_r\end{array}$$

其中$I$是车辆转动惯量。代入小转角假设以及侧偏力假设(两个轮胎)可得

$$\{\begin{array}{l}m{a}_y=2C_{\alpha r}{\alpha }_r+2C_{\alpha f}{\alpha }_f\\ I\ddot{\theta }=2C_{\alpha f}{\alpha }_f{l}_f-2C_{\alpha r}{\alpha }_r{l}_r\end{array}$$

由于前后轮速度与车体速度方向的不一致，产生垂直于车身的相反转矩使车转动，由几何关系可得

$$\{\begin{array}{l}-{\alpha }_r={\delta }_r+\frac{\dot{\theta }{l}_r-\dot{y}}{v_x}\\ -{\alpha }_f={\delta }_f-\frac{\dot{\theta }{l}_f+\dot{y}}{v_x}\end{array}$$

由于横向动力学不考虑控制纵向速度，因此$v_x$为定值。对于固结于车身的惯性系而言，不存在转动关系，因此需要将其转换到非惯性坐标系考察速度的变换情况，可以得到沿车身和垂直车身方向的速度变化量

$$\{\begin{array}{l}\mathrm{d}{v}_x=\mathrm{d}v\cos \theta -v\sin \theta \mathrm{d}\theta \\ \mathrm{d}{v}_y=\mathrm{d}v\sin \theta +v\cos \theta \mathrm{d}\theta \end{array}$$

进一步对时间微分，采用小角度假设并忽略二阶项可得

$$\{\begin{array}{l}{a}_x={\dot{v}}_x-\dot{y}\dot{\theta }\\ a_y=\ddot{y}+v_x\dot{\theta }\end{array}$$

将$a_y$、${\alpha }_r$、${\alpha }_f$代入牛顿方程，考虑到后轮转角可以忽略不计，化简得到

$$\frac{\mathrm{d}}{\mathrm{d}t}\left[\begin{array}{c}y\\ \dot{y}\\ \theta \\ \dot{\theta }\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& \frac{2\left(C_{\alpha r}+C_{\alpha f}\right)}{m{v}_x}& 0& \frac{2\left(C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r\right)}{m{v}_x}-v_x\\ 0& 0& 0& 1\\ 0& \frac{2\left(C_{\alpha f}{l}_f-C_{\alpha r}{l}_r\right)}{I{v}_x}& 0& \frac{2\left(C_{\alpha f}{l}_f^2+C_{\alpha r}{l}_r^2\right)}{I{v}_x}\end{array}\right]\left[\begin{array}{c}y\\ \dot{y}\\ \theta \\ \dot{\theta }\end{array}\right]+\left[\begin{array}{c}0\\ -\frac{2C_{\alpha f}}{m}\\ 0\\ -\frac{2C_{\alpha f}{l}_f}{I}\end{array}\right]{\delta }_f$$

即为车辆基于线性侧偏假设的动力学模型

4 车辆横向动力学的误差形式

下面建立基于误差的横向动力学方程。设横向位移误差和航向角误差分别为$e_1$、$e_2$，道路曲率半径为$R_d$，则期望车辆转向半径也为$R_d$以实现沿路径行驶，所以车辆期望的横摆角速度为${\dot{\theta }}_{\mathrm{d}}=v_x/R_{\mathrm{d}}$，期望的横向加速度为$a_{y,\mathrm{d}}=v_x^2/R_{\mathrm{d}}=v_x{\dot{\theta }}_{\mathrm{d}}$，从而

$${\ddot{e}}_1=a_y-a_{y,\mathrm{d}}=\ddot{y}+v_x\left(\dot{\theta }-{\dot{\theta }}_{\mathrm{d}}\right)$$

积分得到横向速度误差

$${\dot{e}}_1=\dot{y}+v_x\left(\theta -{\theta }_{\mathrm{d}}\right)$$

航向误差则为

$$e_2=\theta -{\theta }_{\mathrm{d}}$$

将第三节的动力学方程代入上述误差表达式化简得到其误差形式

$$\begin{gathered} \frac{\mathrm{d}}{\mathrm{d}t}\left[\begin{array}{c}{e}_1\\ {\dot{e}}_1\\ e_2\\ {\dot{e}}_2\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& \frac{2\left(C_{\alpha r}+C_{\alpha f}\right)}{m{v}_x}& -\frac{2\left(C_{\alpha r}+C_{\alpha f}\right)}{m}& \frac{2\left(C_{\alpha f}{l}_f-C_{\alpha r}{l}_r\right)}{m{v}_x}\\ 0& 0& 0& 1\\ 0& \frac{2\left(C_{\alpha f}{l}_f-C_{\alpha r}{l}_r\right)}{I{v}_x}& \frac{2\left(C_{\alpha r}{l}_r-C_{\alpha f}{l}_f\right)}{I}& \frac{2\left(C_{\alpha f}{l}_f^2+C_{\alpha r}{l}_r^2\right)}{I{v}_x}\end{array}\right]\left[\begin{array}{c}{e}_1\\ {\dot{e}}_1\\ e_2\\ {\dot{e}}_2\end{array}\right] \\ +\left[\begin{array}{c}0\\ -\frac{2C_{\alpha f}}{m}\\ 0\\ -\frac{2C_{\alpha f}{l}_f}{I}\end{array}\right]{\delta }_f+\left[\begin{array}{c}0\\ \frac{2\left(C_{\alpha f}{l}_f-C_{\alpha r}{l}_r\right)}{m{v}_x}-v_x\\ 0\\ \frac{2\left(C_{\alpha f}{l}_f^2+C_{\alpha r}{l}_r^2\right)}{I{v}_x}\end{array}\right]{\dot{\theta }}_{\mathrm{d}} \end{gathered}$$

接着可以基于该方程实现LQR、MPC等控制算法

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇