自动驾驶控制算法---动力学方程

汤姆_布利柏

已于 2023-11-02 21:16:19 修改

阅读量287

点赞数

文章标签：自动驾驶人工智能算法

于 2023-10-31 10:35:27 首次发布

本文链接：https://blog.csdn.net/weixin_46072670/article/details/134123956

版权

１、运动学方程对比动力学方程

　　有了运动学方程就可以做相应的控制算法。但是实际上很少用运动学方程做，因为简化的太多了（假设太多），并且运动学方程是非线性的，v（纵向速度）和δ（前轮转角或方向盘转角）是耦合在一起的，不是解耦的。

　　动力学方程是考虑了轮胎的特性，在选取Frenet坐标系时，可以将纵向控制和横向控制解耦。

$\vec{v}=\frac{ds}{dt}$ $\vec{a_{\tau }}=\frac{d^{2}s}{dt^{2}}$ $a_{n}=\frac{v^{2}}{R}=\frac{v^{2\tan \delta }}{L}$ 　　

　　s、v与aτ相关，aτ与油门／刹车直接相关，可以通过控制油门／刹车来控制aτ，通过aτ进行纵向控制。因此，在纵向控制里只需管油门和刹车就可以了不用管方向盘。

　　an与v，δ有关，横向控制依赖于纵向控制和方向盘，当纵向控制稳定以后，v变化不大，所以an（横向控制）只与方向盘有关。

２、动力学方程

　　采用二自由度车辆动力学方程，因为经典，化繁为简，纵向控制假设是常数（即纵向的速度为常数Ｃ）只管横向控制，符合我们分开控制的思路。而且是在牛顿第二定律受力分析基础上加上了轮胎变形的考虑（比运动学考虑的多）。

　　２.１、轮胎的侧偏特性（轮胎变形的术语）

　　定义Ｆ＝Ｃα，α是侧偏角，Ｆ是侧向力。

　　Ｃ是侧偏刚度（反映轮胎软硬程度的物理量）是轮胎的一个基本属性，做实验得到的值（等价于一个特殊的弹簧是线性关系的，但实际上轮胎是橡胶，并不是一个理想弹簧），侧偏刚度和轮胎的垂向力息息相关，这里认为侧偏刚度是一个常数。

　　左图是正的侧向力得到负的侧偏角，右图为负的侧向力得到正的侧偏角。

　　因此，侧偏刚度一定是负的。

　　tip：侧向力是除地面以外的物体对轮胎产生的力，侧偏力是由侧向力引起的地面侧向反作用力

　　２.２、动力学方程的构建

　　上图的两个方框是车的（自行车模型）前后两个轮子，两个力Ｆ都是侧向力，δ是前轮转角，一般汽车的后轮是不转向的，则后轮的方向和车的轴线（车身坐标系的ｘ轴）基本上是一样的（后轮转角为0°），αr是前轮侧偏角，αf是后轮侧偏角。　　

$\sum F_{y}=ma_{y}\Rightarrow ma_{y}=F_{yf}\cos \delta +F_{yr}$

$\sum M=I\ddot{\varphi }\Rightarrow F_{yf}\cos \delta \cdot a-F_{yr}=I\ddot{\varphi }$

$ma_{y}=F_{yf} +F_{yr}=C_{\alpha f}\alpha_{f}+C_{\alpha r}\alpha _{r}$

$I\ddot{\varphi }=F_{yf} \cdot a-F_{yr}\cdot b=aC_{\alpha f}\alpha _{f}-bC_{\alpha r}\alpha _{r}$

　　第一个方程是受力分析方程（牛二），第二个方程是力矩方程（力矩向量 = 距离向量×F = 转动惯量乘以角加速度）。一般假设前轮转角δ较小，所以cosδ ≈ 1。

　　找出 ay 与 y 的关系，以及 αr ， αf 的具体表达式，因为最终希望通过前轮转角 δ 实现对横向位移 y 以及汽车的横摆角 φ 的控制。

　　（1）、 ay 与 y 的关系

　　 $vy = \dot{y}$ $ay = \ddot{y} + vx\dot{\varphi }$

　　因为车身坐标系不是一个惯性系，所以 ay 的公式有点奇怪，要考虑到非惯性系下的单位矢量的求导。

　　（2）、αr，αf 的具体表达式

　　后轮αr：

　　这里的 $\dot{\varphi }b$ 的结果是由刚体平面运动的速度合成法得到的（我感觉 $\dot{\varphi }b$ 这个向量的方向反了）。αr 比较小，则 $\tan \alpha r =\frac{\dot{\varphi }b-v\sin \beta }{v\cos \beta }=\frac{\dot{\varphi }b-vy}{vx} \approx ar$ 。

　　可以得出 $\alpha r = \frac{vy-\dot{\varphi }b}{vx}$ ，因为 αr（侧偏角）是负的。

　　前轮αf：

　　 $\tan \theta = \frac{\dot{\varphi }a + vy}{vx}$ ， $af = \theta - \delta =\frac{\hat{\varphi }a+vy}{vx} -\delta$ 。αf（侧偏角）也是负的。

　　将αr，αf代入上述公式可以得出动力学方程：

$\begin{pmatrix} \frac{dv_{y}}{dt}\\ \frac{dv_{\dot{\varphi }}}{dt} \end{pmatrix}=\begin{pmatrix} \frac{C_{\alpha f}+C_{\alpha r}}{mv_{x}} &\frac{aC_{\alpha f}-bC_{\alpha r}}{mv_{x}}-v_{x} \\ \frac{aC_{\alpha f}-bC_{\alpha r}}{Iv_{x}} & \frac{a^{2}C_{\alpha f}+b^{2}C_{\alpha r}}{Iv_{x}} \end{pmatrix}\begin{pmatrix} v_{y}\\ \dot{\varphi } \end{pmatrix}+\begin{pmatrix} -\frac{C\alpha f}{m}\\ -\frac{aC\alpha f}{I} \end{pmatrix}\delta$