基于几何模型的控制方法pure pursuit和stanley

基于几何模型的控制方法Pure Pursuit方法和Stanley方法

Pure Pursuit方法（纯跟踪方法）

图1 纯跟踪控制方法

纯跟踪方法的思想就是在参考轨迹上找一个点$P_r=(x_r,y_r)$，通过调整前轮转向角${\delta }_f$让车辆后轴中心通过画圆弧轨迹能够经过点$P_r$。下面是纯跟踪方法前轮转向角的计算过程：
$$\{\begin{array}{rl}& {\delta }_f=\arctan (\frac{L}{R})\\ & R=\frac{l_d}{2\sin \alpha }\\ & \alpha =\arctan (\frac{y_r-y_h}{x_r-x_h})-\psi \end{array}$$
式中，${\delta }_f$表示前轮转向角，为车辆模型的输入，$L$表示车辆的轴距，$R$表示以后轴中心的转弯半径，$l_d$表示春跟踪方法的预瞄距离，$\alpha$表示车辆坐标系下预瞄点的朝向角，$P_r=(x_r,y_r)$和$P_h=(x_h,y_h)$分别表示参考轨迹上的预瞄点和主车位置点。（实际中参考轨迹上预瞄点到主车位置不一定恰好等于预瞄距离$l_d$，需要找匹配点进行插值）

纯跟踪方法推导比较直观，根据图中的几何关系可以推导，可以参考运动学模型的推导用两次正弦定理就可以得到前轮转向角${\delta }_f$与各已知量之间的关系。此外，纯跟踪方法控制的是车辆后轴中心，因此可以直接套用以后轴中心为原点的车辆模型。

Stanley方法

图2 Stanley控制方法

Stanley方法的思想像是两个纯P控制器的组合，一个用于控制航向误差，一个用于控制位置误差，找到参考轨迹上离主车位置$P_h=(x_h,y_h)$最近的匹配点$P_r=(x_r,y_r)$，通过调整前轮转向角${\delta }_f$，让主车与匹配点之间的航向误差${\psi }_r-\psi$和位置误差$e_y$趋于0。
$$\{\begin{array}{rl}& {\delta }_f={\delta }_{\psi }+{\delta }_e\\ & {\delta }_{\psi }={\psi }_r-\psi \\ & {\delta }_e=\arctan (\frac{e_y}{l_d})\\ & e_y=\sqrt{(x_r-x_h{)}^2+(y_r-y_h{)}^2}\\ & l_d=\frac{v}{k}\end{array}$$
式中，${\delta }_f$表示前轮转向角，为车辆模型的输入；${\delta }_{\psi }$表示航向误差引起的转向角控制量；${\delta }_e$表示横向位置误差引起的转向角控制量；$e_y$表示横向位置误差；$l_d$表示预瞄距离，是一个自定义参数。

Stanley方法没有纯跟踪控制方法这么精确的几何模型关系，而是将航向误差${\psi }_r-\psi$和位置误差$e_y$直接体现在了前轮转向角上，所以像纯P控制。航向误差和位置误差确实与前轮转向角并无直接关系，只是这个控制方法这么定义${\delta }_{\psi }$和${\delta }_e$的计算方式而已，从负反馈的角度来说总能收敛到参考线上。

Stanley控制方法不需要推导，主要是了解它的思路。此外，从图2中可以看到，Stanley方法主要是针对前轮进行控制，如果用后轴中心为原点的模型，需要把坐标转换到前轮上。

应用demo

Pure Pursuit和Stanley进行轨迹跟踪的效果如图3和图4所示。纯跟踪方法控制的是后轮，看红色的轨迹；stanley方法控制的是前轮，看绿色轨迹。程序在github仓库。

图3 纯跟踪方法轨迹跟踪效果示意

图4 stanley方法轨迹跟踪效果示意