纯跟踪算法（Pure Pursuit）在差速机器人上的应用

Pure Pursuit 是一种路径跟踪算法。

在给定线速度的前提下，它计算移动的角速度令机器人从其当前位置到达机器人前方的某个前瞻点（lookahead）。该算法根据机器人的当前位置不断地追着它前面的一个点，直到路径的最后一个点。

1.差速轮模型 

差速轮模型中：

      

 2.纯跟踪算法

 图中()是我们下一个要追踪的路点，它位于我们已经规划好的全局路径上，现在需要控制车辆是的车辆的后轴经过该路点，表示车辆当前位置（即后轴位置）到目标路点的距离， 表示目前车身姿态和目标路点的夹角，那么根据正弦定理我们可以推导出如下转换式：

可以得到：

定义距离目标点横向误差为，得到：

联立模型方程得到：

3.ROS速度计算程序

void PurePursuit::computeVelocities(nav_msgs::Odometry odom)
{
  // Get the current robot pose
  geometry_msgs::TransformStamped tf;
  try
  {
    tf = tf_buffer_.lookupTransform(map_frame_id_, robot_frame_id_, ros::Time(0));
    for (; idx_ < path_.poses.size(); idx_++)
    {
      if (distance(path_.poses[idx_].pose.position, tf.transform.translation) > ld_)
      {
        KDL::Frame F_bl_ld = transformToBaseLink(path_.poses[idx_].pose, tf.transform);
        lookahead_.transform.translation.x = F_bl_ld.p.x();
        lookahead_.transform.translation.y = F_bl_ld.p.y();
        lookahead_.transform.translation.z = F_bl_ld.p.z();
        F_bl_ld.M.GetQuaternion(lookahead_.transform.rotation.x,
                                lookahead_.transform.rotation.y,
                                lookahead_.transform.rotation.z,
                                lookahead_.transform.rotation.w);
        break;
      }
    }
    if (!path_.poses.empty() && idx_ >= path_.poses.size())
    {
      KDL::Frame F_bl_end = transformToBaseLink(path_.poses.back().pose, tf.transform);
      if (fabs(F_bl_end.p.x()) <= pos_tol_)
      {
        goal_reached_ = true;
        path_ = nav_msgs::Path();
      }
      else
      {
        // Find the intersection between the circle of radius ld
        // centered at the robot (origin)
        // and the line defined by the last path pose
        double roll, pitch, yaw;
        F_bl_end.M.GetRPY(roll, pitch, yaw);
        double k_end = tan(yaw); // Slope of line defined by the last path pose
        double l_end = F_bl_end.p.y() - k_end * F_bl_end.p.x();
        double a = 1 + k_end * k_end;
        double b = 2 * l_end;
        double c = l_end * l_end - ld_ * ld_; 
        double D = sqrt(b*b - 4*a*c);
        double x_ld = (-b + copysign(D,v_)) / (2*a);
        double y_ld = k_end * x_ld + l_end;
        lookahead_.transform.translation.x = x_ld;
        lookahead_.transform.translation.y = y_ld;
        lookahead_.transform.translation.z = F_bl_end.p.z();
        F_bl_end.M.GetQuaternion(lookahead_.transform.rotation.x,
                                 lookahead_.transform.rotation.y,
                                 lookahead_.transform.rotation.z,
                                 lookahead_.transform.rotation.w);
      }
    }

    if (!goal_reached_)
    {
      // Compute linear velocity.
      // Right now,this is not very smart :)
      v_ = copysign(v_max_, v_);
      // Compute the angular velocity.
      // Lateral error is the y-value of the lookahead point (in base_link frame)
      double yt = lookahead_.transform.translation.y;
      double ld_2 = ld_ * ld_;
      cmd_vel_.angular.z = std::min( 2*v_ / ld_2 * yt, w_max_ );
      // Set linear velocity for tracking.
      cmd_vel_.linear.x = v_;
    }
    else
    {
      // We are at the goal!

      // Stop the vehicle
      
      // The lookahead target is at our current pose.
      lookahead_.transform = geometry_msgs::Transform();
      lookahead_.transform.rotation.w = 1.0;
      
      // Stop moving.
      cmd_vel_.linear.x = 0.0;
      cmd_vel_.angular.z = 0.0;
    }

    // Publish the lookahead target transform.
    lookahead_.header.stamp = ros::Time::now();
    tf_broadcaster_.sendTransform(lookahead_);
    // Publish the velocities
    pub_vel_.publish(cmd_vel_);
  }
  catch (tf2::TransformException &ex)
  {
    ROS_WARN_STREAM(ex.what());
  }
}