后轮位置反馈控制与算法仿真实现

1. 后轮反馈控制

后轮反馈控制（Rear wheel feedback）算法是利用后轮中心的跟踪偏差来进行转向控制量计算的方法，属于Frenet坐标系的一个应用。通过选择合适的李雅普诺夫函数设计控制率，利用后轮中心的跟踪偏差来进行转向控制量计算的方法。

2. 算法原理

后轮反馈控制算法原理如上图所示，其中

• $P$：当前距离车辆最近的路经点；

• $e_y$：$P$点与车辆后轮中心点的横向偏差$AP$，实际上对应的就是frenet坐标下的$l$；

• $\phi$：车辆朝向与$X$轴正方向的夹角，即航向角；

• ${\phi }_r$：$P$点切线与$X$轴正方向的夹角；

• ${\phi }_e$：车辆航向角误差，即$\phi -{\phi }_r$；

• $\overrightarrow{n_{\tau }}$：$P$点法线的单位向量；

• $\overrightarrow{{\tau }_r}$：$P$点切线的单位向量；

• $L$：轴距

• ${\delta }_f$：前轮转角

• $v$：车辆的速度

由前面的文章frenet坐标与cartesian坐标相互转换与代码实现和上图的几何关系可得车辆在参考轨迹上的投影点$P$处的线速度$\dot{s}$和横向误差$e_y$（即对应frenet坐标系下的$\dot{l}$）的表达式为
$$\dot{s}=\frac{\vec{v}\overrightarrow{{\tau }_r}}{1-k_{r}l}=\frac{|\vec{v}||\overrightarrow{{\tau }_r}|cos{\phi }_e}{1-k_r{e}_y}=\frac{|\vec{v}|cos{\phi }_e}{1-k_r{e}_y}\text{\hspace{1em}(1)}$$

$$\dot{e_y}=\dot{l}=\vec{v}\overrightarrow{n_r}=|\vec{v}||\overrightarrow{n_r}|cos(\frac{\pi }{2}-{\phi }_e)=|\vec{v}|sin{\phi }_e\text{\hspace{1em}(2)}$$

由图中几何关系可得航向误差为
$${\phi }_e=\phi -{\phi }_r\text{\hspace{1em}(3)}$$
则航向误差变化率为
$$\dot{{\phi }_e}=\dot{\phi }-\dot{{\phi }_r}=\dot{\phi }-\frac{\dot{s}}{R_r}=\dot{\phi }-\dot{s}{k}_r\text{\hspace{1em}(4)}$$

注：一个刚体的角速度 = 线速度/线速度到速度瞬心的距离，上式中$R_r$为点$P$处的瞬时圆心半径。

将(1)代入(4)可得
$$\dot{{\phi }_e}=\dot{\phi }-\dot{s}{k}_r=\dot{\phi }-\frac{k_r|v}{1-k_r{e}_y}$$