Pure Pursuit 纯追踪法

Big David

已于 2024-03-15 15:22:10 修改

阅读量2.1k

点赞数 3

分类专栏： Control 模块决策规划控制文章标签：自动驾驶笔记算法

于 2023-06-18 17:18:07 首次发布

本文链接：https://blog.csdn.net/BigDavid123/article/details/131271235

版权

决策规划控制同时被 2 个专栏收录

50 篇文章 20 订阅

订阅专栏

Control 模块

9 篇文章 4 订阅

订阅专栏

Pure Pursuit 纯追踪法

适用场景：低速场景(速度过高易产生转弯内切以及超调)

1. 几何学自行车模型

自行车模型
此模型在于简化前轮转向角和后轴将遵循的曲率之间的关系。
$\Large tan\delta =\frac{L}{R}$
该公式能在较低速度的场景下对车辆运动做估计。

2. 纯追踪算法理论分析

从自行车模型出发，纯追踪算法以车后轴为切点，车辆纵向车身为切线。通过控制前轮转角，使车辆可以沿着一条经过目标路点(Gx,Gy)圆弧行驶。
在这里插入图片描述
(Gx,Gy)是下一个需要追踪的路点，位于已经规划好的全局路径上，现在需要控制车辆的后轴经过该路点。

$L_{d}$ : 车辆后轴中心(Cx,Cy)到目标路点(Gx,Gy)的距离，即预瞄距离
$\alpha$ : 目前车身姿态和目标路点的夹角
$R$ : 跟踪的曲率半径
$δ$ : 前轮转角
由正弦定理得：
$\Large\frac{L_{d}}{sin2\alpha } =\frac{R}{sin(\frac{\pi }{2} -\alpha ){\large }}$

继续往下推得 $\large\frac{L_{d}}{2sin\alpha cos\alpha } =\frac{R}{cos\alpha }$ => $\large2R=\frac{L_{d}}{sin\alpha }$

圆弧的曲率: $\large kappa =\frac{1}{R} =\frac{2sin\alpha}{Ld }$

前轮转角 ${\large \delta =arctan(\frac{L}{R} )=arctan(kappa*L){\large } }$

纯追踪算法的最终表达式： ${\Large {\delta (t)=arctan(\frac{2Lsin\alpha (t)}{Ld} )} }$

3. 纯追踪控制器

定义一个新的量e — 车辆当前姿态和目标路点在横向上的误差
在这里插入图片描述
${\large sin\alpha = \frac{e}{L_{d}}{\large }}$

$\large kappa =\frac{1}{R} =\frac{2sin\alpha}{L_{d}}=\frac{2e}{L_{d} ^2}$

=>纯追踪控制器其实是一个横向误差的P控制器 $K_{p} = \frac{2}{Ld ^2}$
这个P控制器受 $L_{d}$ 影响很大，调节预瞄距离成为纯追踪算法的关键。

4. 调节预瞄距离 $L_{d}$

预瞄距离类似驾驶员开车时的视觉跟踪点。直道行驶，驾驶员会选择相对快的速度，视觉跟踪点一般也比较远；弯道行驶时，驾驶员会选择相对慢的速度，视觉跟踪点习惯选择近的位置作为跟踪参考点。
预瞄距离 $L_{d}$ 的计算与当前车辆速度v，曲率kappa存在下面的函数关系：

$L_{d} =f(v,kappa)$

速度越快，预瞄距离越远，可以认为，预瞄距离与速度成正比。
如果前方跟踪曲率较大说明为弯道，需要减少预瞄距离满足弯道行驶的跟踪精度；曲率较小，可以增加预瞄距离保证行驶的平稳性。

所以预瞄距离 $L_{d}$ 与速度 $v$ 和曲率 $ka pp a$ 的关系: $L_{d}\propto \frac{v}{kappa}$

4.1 预瞄距离 $L_{d}$ 的确定

参考文章：基于Pure Pursuit算法的智能车路径跟踪

对于下一时刻将要跟踪的路径中，轨迹曲率K需要进行量化计算。选择一段距离内的轨迹离散化成关键点，进行评估。考虑选择合适长度的距离D内进行计算估计曲折程度。D的计算与车速成正相关。
预瞄距离 $L_{d}$ 与车速 $V$ 的数学关系：
在这里插入图片描述
$V$ :车辆行驶速度
第一项: 车辆制动距离
$\frac{1}{2a_{max}}$
第二项: 车辆遇到异常情况进行反应的车辆行驶距离
第三项: 车辆最小转弯半径