apollo control模块中车身动力学模型推导

声明：本文主要参考了follow轻尘的https://blog.csdn.net/u013914471/article/details/82968608，在此多谢该博主的文章的指引，本文主要是自己学习的过程记录

坐标系背景：车辆纵向为x轴方向，右手坐标系，原点在车辆质心，文章没图

1. 根据牛顿第二定律，结合车辆的质心的受力分析，得出质心在x,y,z方向上的受力分析如下：

在x轴上：(纵向受力可以被无视)

在y轴上：

（1）

力矩平衡结合车辆前后轮受力和受力点到质心的距离可得：

在z轴上：

（2）

其中， 是整车质量，ax为质心在x轴方向受到的加速度，为质心在y轴方向受到的加速度，为前轴中心在x轴方向受到的力，为前轴中心在y轴方向受到的力，为以车辆质心为圆心的转动惯量，为车辆航向角，即为车辆绕质心旋转的角加速度，为前轴中心到质心的距离，为后轴中心到质心的距离。

我们的目的是通过公式（1）（2）求解得到车辆的动力学模型：

为横向加速度，可以分解为横向位移加速度和向心加速度的和，如公式（3）：

（3）

其中，为横向位移，为纵向速度，为绕质心转动的角速度

然后解决和，他们是轮胎受到的侧向力，根据轮胎模型，得前轴受力为：

（4）

其中，为前轮的偏转角，该角度在轮胎悬空的情况下，和转向盘转角成一定的非线性关系，为车辆前轴的纵向速度方向和纵向车身的夹角，两者的差值才是轮胎的侧偏角（此处，apollo开发者卜大鹏以及follow轻尘文章中的解释有误）

后轴受力为：

（5）

其中，和为车辆前后轮胎，每个轮胎的侧偏刚度，所以前后轴各乘以2，为前轮转角，为前轮侧偏角

接下来解决和，tan()为横向速度和纵向速度的比值，而横向速度为车辆位移引起的横向速度和转动引起的横线速度，所以：

（6）

（7）

在小角度下，公式（6）（7）可近似为：

（8）

（9）

至此，把公式（3）（4）（5）（8）（9）带入（1）可得：

所以：

（10）

再把公式（3）（4）（5）（8）（9）带入（2）可得：

（11）

将公式（10）（11）写成矩阵形式就可以得到车辆的动力学模型

 （12）

公式（12）是关于车辆横向位移，横向速度，航向角以及航向角速度的状态方程，但是我们需要更加直接的状态方程对车辆的路径跟踪状态进行反馈，所以，车辆的定位和路径之间的横向误差和航向角误差更能直接明确的反馈路径跟踪的效果。接下来的任务是将公式（12）转换成跟踪误差的状态矩阵。

引入，为横向误差，为横向误差的变化率（横向速度误差），为航向角误差，为航向角误差的变化率（航向角速度误差）。

车辆转过期望角度期望的角速度为：(des = desire)

（13）

所需要的横向加速度为：

（14）

所以横向加速度误差为：

（15）

即：

航向角误差为：

（16）

假设车速短时间恒定的情况下，横向速度误差为：

（17）

这里红色标记存疑，代码是这样计算的

航向角速度误差为

(18)

航向角加速度误差为

(19)

于是，将公式（15）（16）（17）（18）带入公式（10）：

整合一下得：

（20）

然后再将公式（15）（16）（17）（18）（19）带入公式（11）：

即：

（21）

将公式（20）（21）转换成状态空间表达式：

(22)

此处红色貌似省略掉了