49.在ROS中实现local planner（2）- 实现Purepersuit(纯跟踪)算法

48.在ROS中实现local planner（1）- 实现一个可以用的模板实现了一个模板，接下来我们将实现一个简单的纯跟踪控制，也就是沿着固定的路径运动，全局规划已经规划出路径点，基于该路径输出相应的控制速度

1. Pure Pursuit

Pure Pursuit路径跟随便是基于受约束移动机器人圆周运动的特性所开发出来的运动控制方式。原理十分简单，如图所示，移动机器人有一个前视的搜索半径，与机器人规划的路径有一个焦点，假设机器人从当前位置到路径焦点的运动为圆周运动。其中的前视距离便是图1中的L。根据几何关系便可以计算机器人的运动半径。

受约束的机器人模型（不能横向运动）可由两个控制量组成，即运动参考点的线速度v与角速度w。在极短的运行周期中，机器人都是以固定的线速度与角速度运动。因此机器人的运动可以视为圆周运动（w=0时为直线运动）。

2. 运动半径推算

如图所示的机器人便是绕着一个旋转中心进行圆周运动，于是移动机器人的运动控制可视为求解其在运动过程中的实时旋转半径。图中，r为移动机器人的旋转半径。

我们以base坐标系为例，及当前机器人为坐标原点，x轴为前方，y轴为左方，即ROS的坐标系，(x,y)为目标点， L为距离目标点的距离（前视距离），如下计算，容易求得旋转半径r

由图可得 d = r − y d = {r - y} d=r−y
d 2 + x 2 = r 2 d^2+x^2 = r^2 d2+x2=r2
即: r 2 − 2 r y + y 2 + x 2 = r 2 r^2-2ry+y^2+x^2 = r^2 r2−2ry+y2+x2=r2
即: x 2 + y 2 = 2 r y x^2+y^2 = 2ry x2+y2=2ry
即: L 2 = 2 r y L^2 = 2ry L2=2ry
即: r = L 2 / 2 y r = L^2/2y r=L2/2y

即运动半径=前视距离的平方/两倍的y

我们知道r=v/w 即我们只需要给定v/w为固定的值即可

因v与L相关 我们取一次关系
令v=aL a为长数项
可得w=v/r=a*2y/L

3. 坐标转换

我们知道setPlan下发的坐标一般使用的是map坐标系，我们计算的时候需要转换为base坐标系

我们可以使用init接口提供的tf::TransformListener即可完成, （1.14.0版本后接口更新，使用新的接口）

geometry_msgs::PoseStamped PurepursuitPlanner::goalToBaseFrame(const geometry_msgs::PoseStamped& goal_pose_msg) {

#if ROS_VERSION_GE(ROS_VERSION_MAJOR, ROS_VERSION_MINOR, ROS_VERSION_PATCH, 1, 14, 0)

  geometry_msgs::PoseStamped goal_pose, base_pose_msg;

  goal_pose = goal_pose_msg;

  goal_pose.header.stamp = ros::Time(0.0);

  try {

    base_pose_msg = tf_->transform(goal_pose, "base_link");

  } catch (tf2::TransformException& ex) {

    ROS_WARN("transform err");

    return base_pose_msg;

  }

#else

  geometry_msgs::PoseStamped base_pose_msg;

  tf::Stamped<tf::Pose> goal_pose, base_pose;

  poseStampedMsgToTF(goal_pose_msg, goal_pose);

  goal_pose.stamp_ = ros::Time();

  try {

    tf_->transformPose(costmap_ros_->getBaseFrameID(), goal_pose, base_pose);

  } catch (tf::TransformException& ex) {

    ROS_WARN("transform err");

    return base_pose_msg;

  }

  tf::poseStampedTFToMsg(base_pose, base_pose_msg);

#endif

  return base_pose_msg;

}

4. 前视距离

我们不断根据当前位置，更新前视距离，通过前面的接算，给定速度

4.1 前视距离大小设置

前世距离可以根据V我们预设速度相关

• 如果前世距离较大，相当于路径采样间隔较大，跟踪路径与规划路径的偏差会大。

• 如果前世距离较小，机器人容易抖动

4.2 前视距离更新策略

如果当前距离路径中前视距离的点后的n个点的距离小于前世距离，则更新前视距离
即如果当前前视距离的点在路径索引为n，则判断n+m索引距离当前点位置是否小于预设前视距离值

5. 速度限制

一般机器人小车，线速度是>0的即，只能前进，无法后退。这就需要我们新增当前前视点角度判断， 如果角度超过90，即在车的后方。可以对速度修正强制旋转

  auto target_yaw_diff = atan2(goal.pose.position.y, goal.pose.position.x);  // 当前目标点相对机器人的角度
  
.... // 计算半径 速度

    // 当前目标点相对机器人的角度 相差较大(即目标点在机器人后面)， 需要直发角速度(即原地旋转)， 转向目标点
    if (target_yaw_diff > PI*0.5) {
      cmd_vel.linear.x = 0;
      cmd_vel.angular.z = 0.8;
    } else if (target_yaw_diff < -PI*0.5) {
      cmd_vel.linear.x = 0;
      cmd_vel.angular.z = -0.8;
    }

6. 完成判断

我们在前视点到达规划路径的最后一个时，且当前点与该最后一点距离差小于预设的容忍差,强制输出0速度

  if (got && l < GOAL_TOERANCE_XY) {
    goal_reached_ = true;
    cmd_vel.angular.z = 0;
    cmd_vel.linear.x = 0;
  }

7. 测试

• 修改move_base配置文件move_base_params.yaml

# base_local_planner: "dwa_local_planner/DWAPlannerROS"
base_local_planner: pure_pursuit_local_planner/PurepursuitPlanner

dwa_local_planner/DWAPlannerROS—>pure_pursuit_local_planner/PurepursuitPlanner

• 启动模拟器

pibot_simulator

• 启动rviz

pibot_view