路径跟踪算法---Pure Pursuit实现

---

前言

Pure Pursuit是一种基于几何追踪的路径跟踪算法，使车辆或者机器人沿着预定轨迹进行运动，控制方法简单，无需过多考虑运动学和动力学模型，广泛应用于车辆和移动机器人领域，实现路径跟踪。

---

一、Pure Pursuit原理介绍

网上已有很多介绍Pure Pursuit算法原理的文章，读者可以自行了解，本节也给出简单的原理介绍，仅作为参考。
Pure Pursuit算法提出“预瞄点”的概念，通过计算角速度控制机器人从当前位置移动到机器人前方预瞄点，当前位置到预瞄点的距离称为预瞄距离。

如图所示，θ 为机器人转向角，L表示机器人的轴距，R为转向半径，Ld代表预瞄距离，也叫前视距离，α为机器后轴与目标点g连成的向量 与车偏航角之间的夹角。

$$\begin{gathered} tan\theta =L/R=L\ast K \\ \theta =ta{n}^{-1}(L/R)=ta{n}^{-1}L\ast K \end{gathered}$$
K代表曲率。
根据正弦定理：
$$\begin{gathered} Ld/sin(2\alpha )=R/sin(\Pi /2-\alpha ) \\ Ld/2sin\alpha cos\alpha =R/cos\alpha \\ Ld/sin\alpha =2R \end{gathered}$$

可计算出曲率K以及机器人转角θ：
$$\begin{gathered} K=1/R=2sin\alpha /Ld \\ \theta =ta{n}^{-1}L\ast 2sin\alpha /Ld \end{gathered}$$

二、代码实现

   思路 ：通过计算机器人当前位置与目标轨迹的距离，从而找到轨迹上距机器人最小距离目标点的索引，从最小距离的索引开始遍历路径点和预瞄点的距离，若该距离大于预瞄距离，计算机器人当前位置和该路径点的距离和夹角，发布运动控制指令。

根据自己的机器人，定义预瞄距离和轴距

#define PREVIEW_DIS 0.8  //预瞄距离
#define Ld 0.35  //轴距

定义容器，将所有路径点和机器人当前位置的距离保存到容器

std::vector<double> bestPoints_;
    for(int i = 0; i<r_x_.size(); i++)
    {
        float path_x = r_x_[i];
        float path_y = r_y_[i];
        // 遍历所有路径点和当前位置的距离，保存到容器中
        float lad = sqrt(pow(path_x - currentPositionX, 2) +
                        pow(path_y - currentPositionY, 2));
        bestPoints_.push_back(lad);
    }  

查找最小距离以及该距离对应路径点的索引

 //找到数组中最小距离
 auto smallest = min_element(bestPoints_.begin(), bestPoints_.end());
 //找到最小距离的索引
 int index = distance(bestPoints_.begin(),smallest); 

从最小距离的索引开始，遍历之后的路径点与预瞄点的距离，距离比实际预瞄距离大，获取索引
注意考虑到速度的缘故，实际预瞄距离和定义的预瞄距离不符

 int temp_index = 0;    
 //从最小距离的位置开始，遍历路径点 到 实际前视距离的距离
 for(int i = index ; i<r_x_.size(); i++)
 {
     float dis = sqrt(pow(r_y_[index] - r_y_[i], 2) + pow(r_x_[index] - r_x_[i], 2));
     if(dis < preview_dis)
     {
         temp_index = i;
     }
     else
         break;
 }
 index = temp_index;

计算前一步获取索引点与机器人当前位置的距离Ld和偏转角α，进一步求取θ，作为机器人角速度发布。

 float alpha = atan2(r_y_[index] - currentPositionY, r_x_[index] - currentPositionX) - calRPY_[2];
  // 当前点和目标点的距离dl
  float dl = sqrt(pow(r_y_[index] - currentPositionY, 2) +
                  pow(r_x_[index] - currentPositionX, 2));
  // 发布小车运动指令及运动轨迹
  if( dl>0.05 )
  {
      float theta = atan(2 * Ld * sin(alpha) / dl);
      cmdVelPub(0.5,theta); 
   }

---

源码地址：pure_pursuit

效果

放一张效果图，纯路径跟踪算法转弯处会偏离设定的路径，效果不如pid算法好，当然也可能和设计的预瞄距离大小有关。