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

“TEB”全称Time Elastic Band（时间弹性带）Local Planner，该方法针对全局路径规划器生成的初始轨迹进行后续修正(modification)，从而优化机器人的运动轨迹，属于局部路径规划。在轨迹优化过程中，该算法拥有多种优化目标，包括但不限于：整体路径长度、轨迹运行时间、与障碍物的距离、通过中间路径点以及机器人动力学、运动学以及几何约束的符合性。“TEB方法”明确考虑了运动状态下时空方面的动态约束，如机器人的速度和加速度是有限制的。”TEB”被表述为一个多目标优化问题，大多数目标都是局部的，只与一小部分参数相关，因为它们只依赖于几个连续的机器人状态。这种局部结构产生了一个稀疏的系统矩阵，使得它可以使用快速高效的优化技术，例如使用开源框架“g2o”来解决“TEB”问题。

详细理解参考文章：

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