车辆轨迹计算

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/zghforever/article/details/82869519

---

一、基于地面坐标系建模

如下图所示，车辆运动轨迹的计算建立在地面坐标系上。这里定义 $X_1$轴的指向与车辆功能开启时的车体坐标系 $X_2$轴指向一致，即初始状态车体坐标系与地面坐标系重合。定义车辆的初始位置为 $(x_1,y_1,\psi_1)$，下一时刻车辆位置为 $(x_2,y_2,\psi_2)$，转弯半径为 $R$，旋转坐标点为 $(x_0,y_0)$。定义 $R_{01}$在 $X$轴和 $Y$轴上的投影分别为 $H_1$和 $L_1$， $R_{02}$在 $X$轴和 $Y$轴上的投影分别为 $H_2$和 $L_2$。假设车辆在 $\Delta t$内的速度为 $V$，车辆在短时间内可以看作是匀速运动。

---

---

二、角度关系

$\psi_2 = \psi_1 + \Delta\psi$
其中 $\Delta\psi$计算如下：
$\Delta\psi = \frac{V*\Delta t }{R}$

---

三、计算投影

计算 $R_{01}$在 $X$轴和 $Y$轴的投影：
$H_1 = R*\sin(\psi_1)\\ L_1 = R*\cos(\psi_1)$
计算 $R_{02}$在 $X$轴和 $Y$轴的投影：
$H_2 = R*\sin(\psi_2)\\ L_2 = R*\cos(\psi_2)$

---

四、计算车辆位置

$x_2 = x_1 + H_2 - H_1\\ y_2 = y_1 + L_1 - L_2$
带入得：
$x_2 = x_1 + R * (\sin(\psi_2) - \sin(\psi_1))\\ y_2 = y_1 + R * (\cos(\psi_1) - \cos(\psi_2))$

---

五、考虑方向盘转角正负

定义当车辆方向盘转角为负值时，车辆向右运动；当方向盘转角为正值时，车辆向左运动；当方向盘转角为0时，车辆直线行驶。定义方向盘转角为 $\theta$，则 $\theta$的符号可以通过 $sign$函数去获取，可以表示为 $sign(\theta)$。

$x_2 = x_1 + sign(\theta) * R * (\sin(\psi_2) - \sin(\psi_1)) \\ y_2 = y_1 + sign(\theta) * R * (\cos(\psi_1) - \cos(\psi_2))$