基于几何关系的车辆轨迹计算

---

一、基于地面坐标系建模

如下图所示，车辆运动轨迹的计算建立在地面坐标系$O_1{X}_1{Y}_1$上。
定义车辆的$t_1$时刻的位置为$(x_1,y_1)$，偏航角为${\psi }_1$，$t_2$时刻车辆位置为$(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$，正值代表向前行驶，负值代表向后行驶，车辆在短时间内可以看作是匀速运动。

---

---

二、角度关系

假设车辆右向前行驶，则$V$为正值，$R$为负值
$${\psi }_2={\psi }_1+\Delta \psi$$
其中$\Delta \psi$计算如下：
$$\Delta \psi =\frac{V\ast \Delta t}{R}$$

故$\Delta \psi$为负值，及${\psi }_2<{\psi }_1$。

---

三、计算投影

考虑到上述假设车辆往右侧行驶，及$R$为负值，故需加上“$-$”号以代表投影距离值。
计算$R_{01}$在$X$轴和$Y$轴的投影：
$$\begin{gathered} L_1=-R\ast \sin ({\psi }_1) \\ {H}_1=-R\ast \cos ({\psi }_1) \end{gathered}$$
计算$R_{02}$在$X$轴和$Y$轴的投影：
$$\begin{gathered} L_2=-R\ast \sin ({\psi }_2) \\ {H}_2=-R\ast \cos ({\psi }_2) \end{gathered}$$

---

四、计算车辆位置

$$\begin{gathered} x_2=x_1+L_1-L_2 \\ {y}_2=y_1+H_2-H_1 \end{gathered}$$
带入得：
$$\begin{gathered} x_2=x_1+R\ast (\sin ({\psi }_2)-\sin ({\psi }_1)) \\ {y}_2=y_1+R\ast (\cos ({\psi }_1)-\cos ({\psi }_2)) \end{gathered}$$

---

五、考虑相邻两个时刻的转弯半径不同的情况

假设相邻两个时刻的转弯半径分别为$R_1$和$R_2$，
计算$R_1$在$X$轴和$Y$轴的投影：
$$\begin{gathered} L_1=-R_1\ast \sin ({\psi }_1) \\ {H}_1=-R_1\ast \cos ({\psi }_1) \end{gathered}$$
计算$R_2$在$X$轴和$Y$轴的投影：
$$\begin{gathered} L_2=-R_2\ast \sin ({\psi }_2) \\ {H}_2=-R_2\ast \cos ({\psi }_2) \end{gathered}$$
根据几何关系
$$\begin{gathered} x_2=x_1+L_1-L_2 \\ {y}_2=y_1+H_2-H_1 \end{gathered}$$
带入得
$$\begin{gathered} x_2=x_1-R_1\ast \sin ({\psi }_1)+R_2\ast \sin ({\psi }_2) \\ {y}_2=y_1+R_1\ast \cos ({\psi }_1)-R_2\cos ({\psi }_2) \end{gathered}$$