Pure Pursuit 纯追踪法

介绍了几何自行车模型、纯追踪法的理论推导以及预瞄距离的确定

Big David

4890人浏览 · 2023-06-18 17:18:07

Big David · 2023-06-18 17:18:07 发布

Pure Pursuit 纯追踪法

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

1. 几何学自行车模型

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

2. 纯追踪算法理论分析

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

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

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

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

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

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

3. 纯追踪控制器

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

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

=>纯追踪控制器其实是一个横向误差的P控制器 $Kp=2Ld2K_{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$ 的关系: $Ld∝vkappaL_{d}\propto \frac{v}{kappa}$

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

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

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

预瞄距离与车速的函数关系图

上述 $L_{d}$ 的计算规则，作为D的计算公式。通过计算轨迹D距离内，描述kappa。kappa的计算可以由离散路径点的曲率均值或者方差来表示。

确定预瞄距离 $L_{d}$ 后，根据Pure Pursuit算法继续计算出控制参数( $V, W$ )。
前轮转角 $δ\delta$ 对应角速度 $W$ : $W = V / R = k * V$

为了保证车辆在转弯时能过平稳跟踪，需要防止车辆侧翻，根据车辆自身参数对车辆运动过程中的侧向加速度进行限制。

车辆所能承受的最大侧向加速度:
$Accmax=Vmax2RminAcc_{max}=\frac{V_{max}^2}{R_{min}}$
当前控制参数的侧向加速度为： $A cc = W * V$

如果当前控制参数的侧向加速度大于设置的阈值，那么此时的侧向加速度为阈值，并根据侧向加速度和角速度计算出此时的线速度 $V$ ，以保证车辆在转弯过程中的平稳性和舒适性。通常情况下， $Acc_{max}$ 的阈值可以设置较小一点，这样在弯道时速度较低，跟踪的精度会更好一些。

同时，为了保证控制参数平稳，Autoware还加入了低通滤波，防止控制参数突变造成车辆状态变化过快。

4.2 计算流程

纯追踪法的计算流程如下
在这里插入图片描述
设置固定的前视距离和路径曲率肯定无法适应不同的路径，因此就需要对于前视距离的计算方法进行研究改进。

本文详细阐述了几何自行车模型、纯追踪算法的理论推导、以及预瞄距离（有些地方叫前视距离）的确定，希望各位读者批评指正，共同学习进步。

天启AI社区

GitCode 天启AI是一款由 GitCode 团队打造的智能助手，基于先进的LLM（大语言模型）与多智能体 Agent 技术构建，致力于为用户提供高效、智能、多模态的创作与开发支持。它不仅支持自然语言对话，还具备处理文件、生成 PPT、撰写分析报告、开发 Web 应用等多项能力，真正做到“一句话，让 Al帮你完成复杂任务”。

更多推荐