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

? 关于具体的TEB算法理论方面的解释可参考博客https://blog.csdn.net/xiekaikaibing/article/details/83417223以及原作者Christoph R?smann分别在2012年和2013年发表的论文*《Trajectory modification considering dynamic constraints of autonomous robots》、《Efficient trajectory optimization using a sparse model》*。其中指出eletic band（橡皮筋）：连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。关于time eletic band的简述：起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time，即为Timed-Elastic-Band算法。通俗的解释就是TEB生成的局部轨迹由一系列带有时间信息的离散位姿(pose)组成，g2o算法优化的目标即这些离散的位姿，使最终由这些离散位姿组成的轨迹能达到时间最短、距离最短、远离障碍物等目标，同时限制速度与加速度使轨迹满足机器人的运动学。需要指出的是g2o优化的结果并非一定满足约束，即实际都是软约束条件，若参数设置不合理或环境过于苛刻，teb都有可能失败，规划出非常奇怪的轨迹。所以在teb算法中包含有冲突检测的部分，在生成轨迹之后逐点判断轨迹上的点是否与障碍物冲突，此过程考虑机器人的实际轮廓。

? 我们首先要知道teb_local_planner是作为ROS中move_base包的一个插件(plugin)开发的，本身该规划器无法独立作为一个node运行，或者说它只是一个lib，被move_base包调用。故以下先看move_base中调用局部规划器的逻辑。

? *MoveBase::executeCb()是move_base节点在接收到一个全局目标之后的回调函数，其中以controller_frequency_的频率循环调用MoveBase::executeCycle()*函数，该函数中根据特定的规则切换状态机，进行全局路径规划或局部轨迹规划，轨迹规划部分代码如下：

其中变量*tc_*是一个local_planner对象的共享指针(boost::shared_ptr<nav_core::BaseLocalPlanner>)，在move_base类的构造函数中创建和初始化：

这里就用到了move_base的插件(plugin)机制，本质上就是定义了nav_core::BaseLocalPlanner这样一个基类，或者说是“接口(interface)”，实际的local_planner都是该类的派生类(或者说实现了接口)，并且将派生类注册，createInstance函数即ROS的pluginlib库根据传入的名称(类名，例如teb_local_planner)创建了一个实例，之后*tc_*指针实际指向teb_local_planner类的对象。teb_local_planner类的定义与实现在teb_local_planner包的”teb_local_planner_ros.cpp“文件中。

回过头再看*MoveBase::executeCycle()*函数部分，调用local_planner的函数主要是，就是这个函数开始调用teb_local_planner算法，下面转入”teb_local_planner_ros.cpp“文件中。

*TebLocalPlannerROS::computeVelocityCommands()*函数中核心的操作列出如下：

大致流程就是先对全局规划器规划出的*global_plan_*进行一些预处理，包括截取部分路径、转换坐标系，然后更新via_points（在全局路径上等间隔选出），更新障碍物信息（若使用costmap_converter插件，可以将障碍物做一定的简化，简化为线段、多边形等，可在一定程度上提高规划效率），之后开始执行局部轨迹规划，最后保证轨迹是无碰撞的情况下则返回规划结果即机器人需要执行的速度指令。

其中planner_->plan局部轨迹规划的部分最为核心。查看*planner_*变量的信息可见又是一个接口类，teb_local_planner包中实现了两种规划器，一个就是普通的TebOptimalPlanner，另一个是HomotopyClassPlanner，HomotopyClassPlanner是一种同时优化多个轨迹的方法，由于目标函数的非凸性会生成一系列最优的候选轨迹，最终在备选局部解的集合中寻求总体最佳候选轨迹，该工作由Christoph R?smann等人在2017年发表：《Integrated online trajectory planning and optimization in distinctive topologies》，我们暂不讨论。

*TebOptimalPlanner::plan()*函数在”optimal_planner.cpp”文件中实现如下：

其中主要是根据输入的初始路径初始化或更新了时间弹性带(轨迹)的初始状态，设置了轨迹起始点以及速度加速度的约束，最后调用*optimizeTEB()*函数：

这部分的工作就是将轨迹优化问题构建成了一个g2o图优化问题并通过g2o中关于大规模稀疏矩阵的优化算法解决，涉及到构建超图(hyper-graph)，简单来说将机器人位姿和时间间隔描述为节点(nodes)（就是优化中调整的东西），目标函数以及约束函数作为边(edges)，该graph中，每一个约束都为一条edge，并且每条edge允许连接的nodes的数目是不受限制的。

? 关于teb_local_planner的所有参数，在ros的wiki页面已经全部列出：http://wiki.ros.org/teb_local_planner#Parameters。以下将针对我们模拟的全向移动机器人解释其中一些参数配置。

? 首先以下是teb参数配置文件的内容：

其中“Trajectory”部分的参数用于调整轨迹，“global_plan_viapoint_sep”参数调整全局路径上选取的航迹点的间隔，应根据机器人的尺寸大小调整，我们机器人的长宽为0.8*0.5(米)，这里修改为0.3，使航迹点更为紧凑；“max_global_plan_lookahead_dist”参数在TebLocalPlannerROS::transformGlobalPlan()函数中被使用，决定局部规划初始轨迹的最大长度，实际调试发现此参数无需过大，因为局部轨迹在每个控制周期都被更新，实际执行的指令仅是轨迹上第一个点的速度值，这里设置为1.5即可，过长也可能导致优化结果无法有效收敛；“global_plan_prune_distance”参数在TebLocalPlannerROS::pruneGlobalPlan()函数中被使用，因为全局路径是从全局起始点到全局目标点的一条轨迹，而初始的局部路径仅是从机器人当前位置到局部目标点的一小段路径，全局路径裁剪其中一部分即局部路径，该参数决定了从机器人当前位置的后面一定距离开始裁剪；“feasibility_check_no_poses”参数在判断生成的轨迹是否冲突时使用，此时设置为3，即从轨迹起点开始逐个检查轨迹上的3个点，若3个点均不发生碰撞，则认为本次轨迹有效，由于teb优化并非硬约束，这里相当于是轨迹生成之后的一层保障，这个参数因根据机器人的速度和环境复杂程度调整，否则极有可能出现在狭窄环境中走走停停的情况；

“Robot”部分的参数主要是根据机器人的实际情况配置最大速度和最大加速度等，不同模型的机器人（差分驱动、全向移动、阿克曼模型）有不同的配置方法。我们使用全向移动机器人（完整模型），所以需要配置y方向的速度与加速度，并且“min_turning_radius”最小转弯半径设置为0；另外“footprint_model”参数用于配置在优化过程中使用的机器人模型（主要是在计算障碍物距离的过程中），有"point", “circular”, “two_circles”, “line”, "polygon"这几种可选，针对每一种模型都有不同的障碍物距离计算方法，其中"point"模型是最简单的，但准确度也最低，"polygon"多边形模型最复杂，完全考虑到机器人的轮廓形状，计算准确度最高。我们这里选择"polygon"模型，然后需要设置多边形的几何参数（多边形的每一个顶点坐标），与move_base中costmap使用的几何参数一致。

“GoalTolerance”部分的参数设置机器人停止运行的容差，根据实际情况调整。其中“free_goal_vel”参数设置机器人在目标点速度的情况，Fasle为默认最终速度为0，即到目标位置的时候应该是保持静止状态。

“Obstacles”部分的参数用于对环境中障碍物的处理方式，体现在轨迹优化阶段。首先是“costmap_converter_plugin”这个参数，它决定是否使用costmap_converter插件，原始costmap中障碍物全部以“点”来表示，计算机器人到障碍物的距离实际需要计算机器人到每一个“障碍物点”的距离，当环境非常复杂时计算代价会非常大。costmap_converter插件的作用是将障碍物预先表示成线段或多边形的形式，可以在一定程度上减轻后续计算距离的压力，具体介绍可见ROS wiki页面：http://wiki.ros.org/costmap_converter或中文介绍https://www.ncnynl.com/archives/201809/2604.html。但同时这种预处理的方法也会耗费资源，需要根据实际环境的情况来判断是否启用。如果使用的话"costmap_converter::CostmapToPolygonsDBSMCCH"是一个较为精确的方法，它将环境转换为非凸多边形；在将障碍物距离加入g2o优化框架中（障碍物距离是目标函数之一，描述为超图的边），“min_obstacle_dist”参数限制机器人与障碍物的最小距离，实际还配合“obstacle_association_force_inclusion_factor”和“obstacle_association_cutoff_factor”这两个参数生效，参考*TebOptimalPlanner::AddEdgesObstacles()*函数中的如下代码：

距离小于min_obstacle_dist * obstacle_association_force_inclusion_factor值的“障碍物点”，被强制加入优化框架中，距离大于min_obstacle_dist * obstacle_association_cutoff_factor的“障碍物点”被直接抛弃不再考虑，然后在剩余的障碍物点中计算机器人左侧最小距离和右侧最小距离。这三个参数的设置非常重要，需要根据机器人的外形尺寸小心调整，否则极易出现狭窄空间机器人无法通过或优化不收敛的情况。

“Optimization”部分的参数主要是设置优化框架中各部分的权重大小，其中“weight_kinematics_nh”参数应设置较小值，因为我们是完整约束机器人无需限制其运动学约束。

“Homotopy Class Planner”部分的参数与HomotopyClass规划器相关，“enable_homotopy_class_planning”设置False选择不启用。