纯追踪算法

纯追踪算法

自行车模型

  在了解纯追踪算法之前，需要先了解车辆模型，一个简化的自行车模型。

运动学自行车模型和动力学自行车模型

轨迹追踪

目前主流的轨迹跟踪方法有两类，基于几何的追踪方法和基于模型预测的方法，纯追踪就是基于几何追踪的方法。

下图是一个运动学自行车模型：

  它是对阿卡曼转向几何的简化，将车辆四轮模型简化为两轮模型，并且假定车辆只在平面上行驶，那么前轮转角δ、轴距和转向半径将遵循以下关系：
$$tan(\delta )=\frac{L}{R}\text{\hspace{1em}(1)}$$

其中δ是前轮的转向角，L为轴距，R为当前转角下的转向半径。
  从自行车模型出发，纯追踪算法以车后轴为切点、车辆纵向车身为切线，通过控制前轮转向角，使车辆可以沿着一条经过目标路经点的圆弧行驶，如下图所示：

图中（$g_x$,$g_y$)是下一个要跟踪的路径点，它位于已经规划好的全局路径中，需要控制车辆使得车辆的后轴经过该路径点，$l_d$代表车辆当前位置（后轴）到目标路径点的距离，α代表当前车身姿态和目标路径点的夹角，根据正弦定理可以推导出以下公式：

$$\frac{l_d}{sin(2\alpha )}=\frac{R}{sin(\pi /2-\alpha )}$$

$$\frac{l_d}{2sin\alpha cos\alpha }=\frac{R}{cos\alpha }$$

$$\frac{l_d}{sin\alpha }=2R\text{\hspace{1em}(2)}$$

结合(1)(2)两式，可以得到纯追踪算法的控制量δ的最终表达式：

$$\delta (t)=ta{n}^{-1}(\frac{2Lsin(\alpha (t))}{l_d})$$

  这里的公式把时间考虑在内，在知道t时刻的车身和目标路径点的夹角α以及距离目标路径点的前视距离$l_d$的情况下，轴距一定，可以利用上式估计出需要的前轮转角，为了更好的理解纯追踪控制器的原理，定义当前姿态和目标路径点在横向上的误差$e_l$,由此得到夹角α：

$$sin(\alpha )=\frac{e_l}{l_d}$$

转向半径就可以写为：

$$\frac{1}{R}=\frac{2}{l_d^2}{e}_l$$

  $e_l$本质上是横向的误差，由上式可知纯追踪控制器其实是一个横向转角的P控制器，其系数P为$\frac{2}{l_d^2}$,控制器的主要影响因素就是前视距离的大小，通常来说，$l_d$被认为是车速的函数，在不同的车速下需要选择不同的前视距离，一种最常用的方法就是把前视距离表示为车辆纵向速度的线性函数，即$l_d$ = k*$v_x$,那么前轮的转角公式就变为了：
$$\delta (t)=ta{n}^{-1}(\frac{2Lsin(\alpha (t))}{k{v}_x(t)})$$
  那么纯追踪控制器前视距离的调整就变成了系数k的调整，通常来说，会使用最大、最小前视距离来约束前视距离，越大的前视距离意味着轨迹的跟踪越平滑，越小追踪会更精确。
  将前视距离设置为速度的函数只是一种常见的方法，前视距离也有可能和其他因素有关，所以纯追踪调参的难点就在前视距离的选取上，很短的前视距离会造成车辆控制的不稳定甚至震荡，为了确保车辆稳定设置较长的前视距离又会出现车辆在大转角处转向不足的问题。
  本文使用Matlab和Simulink构建了一个简单的纯追踪模型并进行了仿真,整体流程框图如下：

  主要分为四部分，第一部分是一个自定义的最邻近路径点搜索函数，中间是控制部分，主要计算车辆横向误差$e_l$和控制量转向角δ，第三部分是车辆模型，使用的Simulink自带的三自由度车辆模型，最后是一个画图的函数,控制部分内部流程框图如下：

最后追踪的效果如下：

github地址：Pure-Pursuit-Control

参考：
http://blog.csdn.net/adamshan/article/details/78696874
https://blog.csdn.net/AdamShan/article/details/80555174?spm=1001.2014.3001.5501
《无人驾驶原理与实践》 申泽邦