纯跟踪算法（Pure Pursuit）在差速机器人上的应用-Toy模板网

这篇具有很好参考价值的文章主要介绍了纯跟踪算法（Pure Pursuit）在差速机器人上的应用。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

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

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

1.差速轮模型

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

差速轮模型中：

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

2.纯跟踪算法

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

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

可以得到：

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

联立模型方程得到：文章来源地址https://www.toymoban.com/news/detail-506872.html

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());
  }
}

到了这里，关于纯跟踪算法（Pure Pursuit）在差速机器人上的应用的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！