pure pursuit预瞄距离的改进思路方法介绍

目录

预瞄距离计算规则介绍

数学模型抽象及流程

小结

---

预瞄距离计算规则介绍 

根据上一篇的介绍，pure pursuit跟踪算法，其跟踪性能很大程度上取决于前视距离L的影响。在不同情况下，为了达到较好的跟踪效果，前视距离L是需要动态调整的。为此，在本篇内容中，介绍前视距离L的计算规则来达到较好的跟踪性能。

在轨迹跟踪中，其前视距离类似于人类自己开车时所选择的视觉跟踪点，主要考虑在下一段将要行驶路段的状况，包括道路情况，交通、速度等。例如：在行驶在直道时，我们会选择较高的速度，并且习惯考虑前方较远的点作为跟踪参考点；在行驶在弯道时，就会选择减速，习惯选择较近的点作为跟踪参考点。我们将这一过程进行抽象简化，那么可以认为：

预瞄距离L的计算与当前车辆速度V，跟踪轨迹的曲率K，存在某种的函数关系,用下式来表示： 

                                                        

数学模型抽象及流程 

（1）预瞄距离与车速的关系；

车辆行驶速度在很大程度上会影响预瞄距离，速度越快，为了保证安全性以及车辆运行的平稳性，那么需要的预瞄距离就越远。可以认为，预瞄距离与速度V成正比。即：

                                                           

（2）预瞄距离与跟踪轨迹曲率的关系； 

跟踪轨迹的曲率是影响预瞄距离计算的重要因素之一，如果前方跟踪轨迹曲率较大说明前方道路为弯道，此时需要减少预瞄距离以满足对弯道行驶时的跟踪精度；相反的，曲率较小说明前方近似直道，此时可增加预瞄距离以保证直到行驶的平稳性。因此，跟踪轨迹的曲率K与预瞄距离成反比关系。即：

                                                          

因此，可以得出，预瞄距离与速度和轨迹曲率存在如下关系：

                                                          

（3） 跟踪轨迹曲率K计算；

对于下一时刻将要跟踪的路径中，其轨迹的曲折程度即这里的轨迹曲率K需要进行量化计算。我们通过选择一段距离内的轨迹进行离散化成关键点，然后进行评估。那么，在这里也需要考虑选择合适长度的距离D内进行计算估计曲折程度。D的计算与车速成正相关，我们可以参考下面的一段描述：

 根据《基于pure pursuit 算法的智能车路径跟踪》论文中的描述：

 即：上述中的ld的计算准则，可以作为这里的D的计算公式。当然，也可以选择其他的方式来确定D，例如分段速度函数等；通过计算轨迹D距离内，描述其轨迹曲折程度K。K的计算可以利用相邻离散路径点的曲率的均值或者方差来表示。

通过上述方式，可以获得下一段跟踪轨迹的曲折情况。

那么，预瞄距离L的计算函数，,为系数，其需要手动调整确定合适的值。

在确定了预瞄距离L之后，就根据pure pursuit 算法的相关计算公式继续进行计算，得出相应的控制参数（V，W）。

同时，为了保证车辆在转弯时能过平稳跟踪，需要防止车辆侧翻，根据车辆自身参数对车辆运动过程中的侧向加速度进行限制。同时，为了保证控制参数平稳，需要加入低通滤波，防止控制参数突变造成车辆状态变化过快。即在Autoware中，twistfilter节点的使用，

根据车辆最大速度及最小转弯半径，可大致计算出车辆所能承受的最大侧向加速度，计算公式如下：

        那么，通过对当前控制指令的计算的到此时的侧向加速度值，并进行计算处理。

当前控制参数的侧向加速度计算公式为：

                                                                   

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

        整个计算流程如下：

小结

下面分别展示，在直线、弯道、直角弯、U型弯下的轨迹跟踪效果。其中，测试过程中设置车辆运动速度为2m/s，路径点离散化采样间隔为0.5m。在图中，绿色的球体即为计算的跟踪预瞄点；红色的点为规划好的路径点，需要控制车辆沿着路经点运动；白色的轨迹为轨迹跟踪算法计算出的车辆将要运行的轨迹。

 从实验结果上看，加入Ld计算规则对前视距离进行动态调整后，车辆能够较好的跟踪不同的轨迹，整体的跟踪性能有较大的提升。对于低速（小于5m/s）时，测试效果还可以；中速（小于10m/s）时只进行过仿真测试，Ld都能够根据轨迹进行动态调整，但是没进行过实车测试，具体性能不好评估。