TEB算法详解（TebLocalPlannerROS::computeVelocityCommands（1））

详细理解参考文章：

1、听说现在自动驾驶很火，所以我也做了一个
2、TEB算法１－teb原理详解

论文原文：Trajectory modification considering dynamic constraints of autonomous robots

论文原文翻译：TEB论文翻译

源码下载：teb_local_planner

TebLocalPlannerROS::computeVelocityCommands作为teb算法与move_base算法的接口函数，是作为teb算法的重点部分去分析的，这一节先看该算法的前半部分，也就是局部路径规划前的数据处理部分，这部分算法的基本思路如下：

1、获取机器人坐标与速度

teb要进行路径规划，首先需要知道自己的位置，同时作为运动控制算法也需要获取当前的速度：

// Get robot pose
  // 获得机器人位姿 二维平面：x y thea
  geometry_msgs::PoseStamped robot_pose;
  costmap_ros_->getRobotPose(robot_pose);
  robot_pose_ = PoseSE2(robot_pose.pose);
    
  // Get robot velocity
  // 根据odom订阅的消息计算速度
  geometry_msgs::PoseStamped robot_vel_tf;
  odom_helper_.getRobotVel(robot_vel_tf);
  robot_vel_.linear.x = robot_vel_tf.pose.position.x;
  robot_vel_.linear.y = robot_vel_tf.pose.position.y;
  robot_vel_.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation);

这里的getRobotVel返回的是根据odom获得的机器人当前X、Y方向的线速度以及角速度

2、路径裁减

在获取完当前机器人的状态后，下一步就是对机器人的全局路径进行裁减，因为前面全局路径规划完成了一条全局路径的规划，但是这个路径的起点肯定是要随着机器人运动而变化的，对于过去的一些路径点，需要将其从路径点中剔除掉：

	// prune global plan to cut off parts of the past (spatially before the robot)
  // 截取全局路径的一部分作为局部规划的初始轨迹，主要是裁切掉机器人后方的一部分路径
  pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance);

这里的处理方式也很简单，就是从全局路径规划容器的起点开始一个个遍历，与当前机器人的全局坐标做差，距离超过一定阈值的点删除掉。阈值使用的是外部参数：global_plan_prune_distance。

3、从全局路径规划容器选取局部路径规划点

裁减完全局路径规划容器后，下一步会从全局路径规划点中选择一部分点位作为局部路径规划的点位并且将这些点转换到局部路径规划的frame_id下。

首先考虑如何选取这些点位：点位数量不会太多，要不然局部路径规划的计算量会很大而且太远的点也没有太大意义。那么距离该如何选择？这里使用的是以下几种判定条件：

	while(i  (int)global_plan.size() && sq_dist  sq_dist_threshold && (max_plan_length0 || plan_length  max_plan_length))
    {
      const geometry_msgs::PoseStamped& pose = global_plan[i];
      tf2::doTransform(pose, newer_pose, plan_to_global_transform);

      transformed_plan.push_back(newer_pose);

      double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
      double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
      sq_dist = x_diff * x_diff + y_diff * y_diff;
      
      // caclulate distance to previous pose
      if (i>0 && max_plan_length>0)
        plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position);

      ++i;
    }

上面这个函数将全局路径规划中的每个点push到transformed_plan容器中，跳出while的情况包括：

1、全局路径规划容器内所有点已经遍历完

这种情况一般会在靠近目标点附近，正常来说这个条件触发的可能性不大

2、全局点与机器人的距离超过阈值

条件2：

sq_dist  sq_dist_threshold

sq_dist 是当前全局规划点距离机器人的距离平方和，而sq_dist_threshold来自于：

	//we'll discard points on the plan that are outside the local costmap
    double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
                                     costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
    dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are
                           // located on the border of the local costmap
    

    int i = 0;
    double sq_dist_threshold = dist_threshold * dist_threshold;

所以这个值是跟costmap大小相关的一个参数，这是一个以机器人为中心，costmap大小乘以0.85为半径的圆。dwa中做规划的时候取局部路径点似乎也是这个套路。

3、取值的数量超过设置的阈值

条件3：

(max_plan_length0 || plan_length  max_plan_length)

其中的max_plan_length来自于参数设置：cfg_.trajectory.max_global_plan_lookahead_dist。而plan_length则是一个距离值，它来自于全局路径规划点中每个点之间的距离和，即代表了需要规划的总路径长度：

if (i>0 && max_plan_length>0)
        plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position);

然后在选取出这些点位后，对点位进行frame_id的转换：

	if (transformed_plan.empty())
    {
      tf2::doTransform(global_plan.back(), newer_pose, plan_to_global_transform);

      transformed_plan.push_back(newer_pose);
      
      // Return the index of the current goal point (inside the distance threshold)
      if (current_goal_idx) *current_goal_idx = int(global_plan.size())-1;
    }

因此，这步执行完成后机器人得到了局部路径规划需要的点位。

4、对局部路径规划点进行筛选

上一步获得全局路径规划点后，下一步对这些点进行了一定的筛选：

// update via-points container
  // 更新via-points容器
  //via_points_.clear遍历transformed_plan将其中二点间隔大于global_plan_viapoint_sep的点加入到 ViaPointContainer via_points_;
  if (!custom_via_points_active_)
    updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);

这个函数的主要作用是遍历当前局部路径规划容器，按照cfg_.trajectory.global_plan_viapoint_sep参数对点进行筛选，如果两个点之间的距离小于该值则不做处理，超过阈值的点加入到via_points_中。

for (std::size_t i=1; i  transformed_plan.size(); ++i) // skip first one, since we do not need any point before the first min_separation [m]
  {
    // check separation to the previous via-point inserted
    if (distance_points2d( transformed_plan[prev_idx].pose.position, transformed_plan[i].pose.position )  min_separation)
      continue;
    // add via-point
    via_points_.push_back( Eigen::Vector2d( transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y ) );
    prev_idx = i;
  }

5、检查是否到达目标点

在完成路径点的提取后，下一步判断当前机器人坐标与目标点距离，从逻辑上来讲只要机器人的坐标与角度与目标点的差值小于阈值就可以了，但是实际上这里还增加了几个判定条件：

if(fabs(std::sqrt(dx*dx+dy*dy))  cfg_.goal_tolerance.xy_goal_tolerance
    && fabs(delta_orient)  cfg_.goal_tolerance.yaw_goal_tolerance
    && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0)
    && (base_local_planner::stopped(base_odom, cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel)
        || cfg_.goal_tolerance.free_goal_vel))
  {
    goal_reached_ = true;
    return mbf_msgs::ExePathResult::SUCCESS;
  }

上述判断中一共使用到了6个设定参数：

cfg_.goal_tolerance.xy_goal_tolerance
cfg_.goal_tolerance.yaw_goal_tolerance
cfg_.goal_tolerance.complete_global_plan
cfg_.goal_tolerance.theta_stopped_vel
cfg_.goal_tolerance.trans_stopped_vel
cfg_.goal_tolerance.free_goal_vel

这里前面两个是用来做与目标点的距离以及角度阈值判断的，第三个是否完全到位判断，但是它同时受via_points_.size()影响，似乎设置true或者false影响不会太大，最后三个应该是判断到位且速度是否停止。即判断机器人当前是否到达目标点的判定不仅仅看机器人当前位姿与目标点的位姿关系，同时还判断了当前的速度关系。如果都满足要求才会返回成功。

6、规划恢复

规划恢复顾名思义就是故障情况下的恢复，这里的故障存在两种情况：

6.1、短期内的规划失败

第一种情况短期内的规划失败是指当机器人规划失败时进入恢复模式，此时机器人没有到达目标点，但是距离上次局部路径规划成功的时间间隔很小：

	// reduced horizon backup mode
    if (cfg_.recovery.shrink_horizon_backup && 
        goal_idx  (int)transformed_plan.size()-1 && // we do not reduce if the goal is already selected (because the orientation might change -> can introduce oscillations)
       (no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).toSec()  cfg_.recovery.shrink_horizon_min_duration )) // keep short horizon for at least a few seconds
    {
        ROS_INFO_COND(no_infeasible_plans_==1, "Activating reduced horizon backup mode for at least %.2f sec (infeasible trajectory detected).", cfg_.recovery.shrink_horizon_min_duration);
        // Shorten horizon if requested
        // reduce to 50 percent:
        int horizon_reduction = goal_idx/2;        
        if (no_infeasible_plans_ > 9)
        {
            ROS_INFO_COND(no_infeasible_plans_==10, "Infeasible trajectory detected 10 times in a row: further reducing horizon...");
            horizon_reduction /= 2;
        }
        // we have a small overhead here, since we already transformed 50% more of the trajectory.
        // But that's ok for now, since we do not need to make transformGlobalPlan more complex 
        // and a reduced horizon should occur just rarely.
        int new_goal_idx_transformed_plan = int(transformed_plan.size()) - horizon_reduction - 1;
        goal_idx -= horizon_reduction;
        if (new_goal_idx_transformed_plan>0 && goal_idx >= 0)
            transformed_plan.erase(transformed_plan.begin()+new_goal_idx_transformed_plan, transformed_plan.end());
        else
            goal_idx += horizon_reduction; // this should not happen, but safety first ;-) 
    }

在这种情况下，算法会缩小规划范围，以更近的点作为规划目标，尝试重新规划出可行路径。goal_idx记录了目前local plan优化的在global plan中最远waypoint的index。此时算法会对局部路径点进行裁减，将远处的点先裁减掉，进行一个近距离的路径规划。

6.2、长期的规划失败

当多次短期规划都失败了的话，算法会进入这部分。这部分代码用于判断机器人是否长时间在某个地方发生振荡：。具体步骤如下：

首先将机器人当前速度与角速度归一化到[0,1]：

//将当前速度与角速度归一化到【0-1】
failure_detector_.update(last_cmd_, cfg_.robot.max_vel_x, cfg_.robot.max_vel_x_backwards, max_vel_theta,
                               cfg_.recovery.oscillation_v_eps, cfg_.recovery.oscillation_omega_eps);

void FailureDetector::update(const geometry_msgs::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps)
{
    if (buffer_.capacity() == 0)
        return;
    
    VelMeasurement measurement;
    //这里仅记录速度x
    measurement.v = twist.linear.x; // just consider linear velocity in x-direction in the robot frame for now
    measurement.omega = twist.angular.z;
    //归一化速度到(0,1)区间
    if (measurement.v > 0 && v_max>0)
        measurement.v /= v_max;
    else if (measurement.v  0 && v_backwards_max > 0)
        measurement.v /= v_backwards_max;
    
    if (omega_max > 0)
        measurement.omega /= omega_max;
    // 存入buffer_中
    buffer_.push_back(measurement);
    // immediately compute new state
    detect(v_eps, omega_eps);
}

然后对线速度与角速度进行判断。这里提到了上述一个容器buffer_。这里面存储了一段时间内的机器人线速度与角速度，判断机器人当前是否处于振荡的条件是：
1、机器人在一段时间内的平均线速度与角速度都小于阈值。
2、机器人在前后两次采样过程中机器人的角速度相反。
这个意思就是说机器人在某个位置不走，还角速度正负来来回回摆动，则我们认为机器人处于振荡状态。

//检查机器人是否震荡
bool FailureDetector::detect(double v_eps, double omega_eps)
{
    oscillating_ = false;
    
    if (buffer_.size()  buffer_.capacity()/2) // we start detecting only as soon as we have the buffer filled at least half
        return false;
    //buffer_中存储了每一次的角速度与线速度
    double n = (double)buffer_.size();
            
    // compute mean for v and omega
    double v_mean=0;
    double omega_mean=0;
    int omega_zero_crossings = 0;
    for (int i=0; i  n; ++i)
    {
        v_mean += buffer_[i].v;
        omega_mean += buffer_[i].omega;
        //前后两个角速度方向是否一致
        if ( i>0 && g2o::sign(buffer_[i].omega) != g2o::sign(buffer_[i-1].omega) )
            ++omega_zero_crossings;
    }
    v_mean /= n;
    omega_mean /= n;
    //如果线速度和角速度均值小于阈值，且方向震荡，则判定机器人处于震荡状态
    if (std::abs(v_mean)  v_eps && std::abs(omega_mean)  omega_eps && omega_zero_crossings>1 ) 
    {
        oscillating_ = true;
    }
//     ROS_INFO_STREAM("v: " 
    return oscillating_;
}

当机器人处于振荡状态，同时如果这个时间超过阈值cfg_.recovery.oscillation_recovery_min_duration，则机器人会进行旋转，旋转方向由机器人当前角速度反方向决定，如果没有超过阈值则以当前机器人角速度方向进行旋转。

7、调整局部路径规划终点角度

对于局部路径规划的终点角度，通过参数cfg_.trajectory.global_plan_overwrite_orientation来决定是否修改，设置为true时会修改终点角度，flase时不进行修改。

7.1、修改局部路径规划终点角度

当需要修改终点角度时，会进行判断：当前局部路径规划的终点与全局路径规划的终点是否接近，如果两个点比较接近(就是同一个点或者下一个点就是终点)则直接使用当前局部路径规划中终点的方向作为规划的方向。

// 检查当前是否靠近目标点，moving_average_length默认为3
  if (current_goal_idx > n-moving_average_length-2)
  {
    if (current_goal_idx >= n-1) // we've exactly reached the goal
    {
      return tf2::getYaw(local_goal.pose.orientation);
    }
    else
    {
      tf2::Quaternion global_orientation;
      tf2::convert(global_plan.back().pose.orientation, global_orientation);
      tf2::Quaternion rotation;
      tf2::convert(tf_plan_to_global.transform.rotation, rotation);
      // TODO(roesmann): avoid conversion to tf2::Quaternion
      return tf2::getYaw(rotation *  global_orientation);
    }     
  }

如果当前局部路径规划的终点跟全局目标点之间还间隔很多个点的话，会使用当前局部路径的终点以及向后两个点的角度进行平滑处理，得到一个新的角度值。

	std::vectordouble> candidates;
  geometry_msgs::PoseStamped tf_pose_k = local_goal;
  geometry_msgs::PoseStamped tf_pose_kp1;
  
  int range_end = current_goal_idx + moving_average_length;
  for (int i = current_goal_idx; i  range_end; ++i)
  {
    // Transform pose of the global plan to the planning frame
    tf2::doTransform(global_plan.at(i+1), tf_pose_kp1, tf_plan_to_global);

    // calculate yaw angle  
    candidates.push_back( std::atan2(tf_pose_kp1.pose.position.y - tf_pose_k.pose.position.y,
        tf_pose_kp1.pose.position.x - tf_pose_k.pose.position.x ) );
    
    if (irange_end-1) 
      tf_pose_k = tf_pose_kp1;
  }
  return average_angles(candidates);

7.2、不修改局部路径规划终点角度

不修改局部路径规划终点角度的话终点的角度就是局部路径规划最后一个点的角度：

//根据当前local planer 的goal的后面几个waypoint计算平均角度，用这个角度来重写当前local  planer 的goal的角度
  if (cfg_.trajectory.global_plan_overwrite_orientation)
  {
    robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, transformed_plan.back(), goal_idx, tf_plan_to_global);
    // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization)
    //  (enable using the plan as initialization) 用真实的目标点朝向覆盖transformed_plan的目标点朝向
    tf2::Quaternion q;
    q.setRPY(0, 0, robot_goal_.theta());
    tf2::convert(q, transformed_plan.back().pose.orientation);
  }  
  else
  {
    //直接使用全局路径的朝向
    robot_goal_.theta() = tf2::getYaw(transformed_plan.back().pose.orientation);
  }

8、插入当前机器人位姿

在确认完终点后，下一步需要给定起始点，前面虽然根据全局路径规划中截取了局部路径规划的点，但是这个容器的起点未必是机器人当前时刻所处的点，因此这里重新插入一下当前的点位作为机器人的起点：

 // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory)
  // 用真实的起始点位置覆盖transformed_plan的起始点位置 (allows using the plan as initial trajectory)
  if (transformed_plan.size()==1) // plan only contains the goal// 路径中只有目标点
  {// 插入起始位姿（还没有初始化）
    transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // insert start (not yet initialized)
  }// 更新起始点，将当前位置设置为plan的初始第一个位姿
  transformed_plan.front() = robot_pose; // update start

9、更新障碍物

确认完路径起点跟终点后，下一步就是确定局部地图中的障碍物信息。障碍物信息存储在obstacles_容器中，根据下述函数更新：

// 用代价地图信息或者costmap_converter提供的多边形信息来更新障碍物容器
  if (costmap_converter_)
    updateObstacleContainerWithCostmapConverter();
  else
    updateObstacleContainerWithCostmap();

先看上面updateObstacleContainerWithCostmapConverter函数，这里主要调用了costmap_converter_->getObstacles()函数，并对障碍物进行了一定的分类，然后将障碍物坐标存储到obstacles_容器中。而updateObstacleContainerWithCostmap函数则是跟cfg_.obstacles.include_costmap_obstacles参数配套使用，用于添加配置文件中设置好的障碍物坐标参数。

除了上述两个更新方式之外，障碍物的更新还可以通过订阅topic的方式更新：

  // also consider custom obstacles (must be called after other updates, since the container is not cleared)
  // 也考虑自定义障碍物，必须在其他的更新后在被调用，因为该容器没有被清理
  updateObstacleContainerWithCustomObstacles();

这里的updateObstacleContainerWithCustomObstacles函数处理了来自于custom_obstacle_msg_中的障碍物参数，而这个障碍物参数来自于订阅：

// setup callback for custom obstacles
    custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);

void TebLocalPlannerROS::customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg)
{
  boost::mutex::scoped_lock l(custom_obst_mutex_);
  custom_obstacle_msg_ = *obst_msg;  
}

所以障碍物的更新最终来自于三个地方：costmap地图本身、cfg参数内的设置以及obstacles话题的输出。

到这儿前期的处理工作就基本完成了，后面再看具体的规划过程。

参考：

https://blog.csdn.net/Draonly/article/details/125125173

https://blog.csdn.net/zz123456zzss/article/details/104692548?spm=1001.2101.3001.6650.4&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-4-104692548-blog-124563409.pc_relevant_3mothn_strategy_recovery&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-4-104692548-blog-124563409.pc_relevant_3mothn_strategy_recovery&utm_relevant_index=6