本文转自 https://www.knightdusk.cn/2019/06/features-and-tuning-guide-for-teb-local-planner/
高手zxy的技术小站（做个备份记录）
本文将介绍ROS导航中的局部规划器TEB Local Planner在实际车辆上的应用条件和参数调试方法。同时我们也将会讨论与之相关的局部耗费地图（local costmap）和耗费地图转换器(costmap converter)。这里我们将不会详细讨论如何在ROS导航栈中配置TEB Local Planner。读者若需相关信息可阅读其它文章。
TEB Local planner 的基本特性和使用TEB的主要原因
在讨论我们为何要使用TEB Local Planner之前，首先向读者简要介绍使用TEB Local Planner的理由。毕竟，若是更加简单的局部路径规划器可以满足要求，读者便没有必要再使用计算量大又难以调试的TEB规划器了。
TEB Local Planner的重要特点
TEB规划器可以处理阿克曼底盘车辆的运动学约束。阿克曼底盘即指常见的汽车底盘，一对车轮方向不可变化，一对可以转向。ROS导航栈默认的局部规划器base local planner只能处理万向轮或差速转向的车辆模型。
TEB是时间最短的最优控制器。在最大速度和加速度等参数约束的条件下，TEB规划器将规划出时间最短路径和速度指令。路径上可以表现为明显的切弯、靠近障碍物；速度指令上将会表现为速度和方向指令的快速振荡。
TEB基于姿态搜索最优方案。TEB的基本思路是在车辆附近空间内散布大量的姿态，然后通过搜索树搜索最优方案。因此，车辆轮廓的准确描述对于TEB的工作性能有显著影响。
在系统中使用TEB Local Planner的缺点与潜在问题
TEB规划器对电机和舵机的动态响应性能要求很高。在给定加速度约束的情况下，TEB规划输出，即电机控制器的输入是一个斜坡输入。使用直流电机时，需要认真调试PID参数才能实现比较好的效果。
TEB规划器计算量很高。经笔者尝试，TEB规划器在一款i3-7100U的小型工控机上无法发挥良好效果，默认设置下规划周期可达1秒以上。若开启不同拓扑结构路径的同时规划（homotopy class planning），规划周期甚至可达20秒以上，完全无法使用。
TEB规划器的稳定性一般。由于不断重新规划路径，且实际车辆的定位测速不可能没有误差，在车辆静止时可以观察到舵机抖动。尽管降低规划频率(此频率由move_base的配置确定)有助于改善这一问题，但这同样导致车辆动态避障的性能大大降低。
TEB Local Planner的参数调试方法
下面我们正式开始讨论如何确定TEB Local Planner的各项参数值。本文并非官方文档的简单重复，因此不会详细解释各个参数的含义。如有需要，请参考官方文档。
-
max_vel_x和acc_lim_x 这是速度与加速度约束。注意加速度同样约束减速过程。若电机性能良好且速度不快可以立即刹车，可直接将acc_lim_x设置为0，表示没有约束。若电机不能承受阶跃输入或者响应时间很长，则应当设置加速度限制。
-
max_vel_x_backwards 最大倒车速度。将此速度设置为0或者负数将导致错误。禁止倒车应在penalty部分将前向行驶的权重设置得极高。后文将讨论此问题。
-
max_vel_y和acc_lim_y y轴方向，即垂直于车辆正对方向的速度。对阿克曼底盘的车辆没有意义。
-
wheelbase和cmd_angle_instead_rotvel 后者设置为true时，ROS话题（rostopic） cmd_vel/angular/z 内的数据是舵机角度，其值满足- \pi/2 <z< \pi/2 −π/2<z<π/2 此时应设置wheelbase即前后轮距离。前后轮距离d与速度、角速度和舵机角度之间的关系是：\varphi = arctan(d\omega /v)φ=arctan(dω/v)（注：车辆运动学中心是后轮中点，速度除以角速度所得半径也是以这里为准的）
-
min_turning_radius 最小转弯半径。为避免规划出不可能实现的移动路径，请动手测量实际车辆的转弯半径。此参数事实上约束了舵机的最大转角。有些车辆转向性能不佳，前轮实际转过角度小于舵机角度，则应当给指令转角乘上一增益后再控制舵机，否则车辆将总是不能实现设置的最小转弯半径。请注意此项应当与最大角速度配合设置。
-
max_vel_theta和acc_lim_theta 角速度和角加速度约束。正确配置下最小转弯半径应在低速时生效，而角速度限制应作用在车辆高速行驶时。角速度约束可以防止高重心转弯过快翻车，角加速度限制可以避免车轮打滑，车辆失控。对于低速平稳运行的车辆可以不约束此两项，将二者设为一个足够大的值即可。
-
xy_goal_tolerance和yaw_goal_tolerance 目标跟踪误差。根据车辆运行精度设置。例如，笔者使用一台攀爬RC车构建导航系统，这种车辆转向间隙特性很大，则不应设置严格的航向限制。
-
free_goal_vel 自由目标速度。设为False时，车辆到达终点时的目标速度为0。我们已经指出，TEB是时间最优规划器。缺少目标速度约束将导致车辆“全速冲线”，即使前方有一堵墙也是如此（因为撞墙的时刻不在规划器考虑范围内了）。不过此选项在竞速比赛中比较有用。
-
dt_ref和dt_hysteresis 前文指出，TEB通过状态搜索树寻找最优路径，而dt_ref则是最优路径上的两个相邻姿态（即位置、速度、航向信息，可通过TEB可视化在rivz中看到）的默认距离。此距离不固定，规划器自动根据速度大小调整这一距离，速度越大，相邻距离自然越大。笔者认为，将dt_ref设置为与车辆长度大致是可行的。较小的值理论上可提供更高精度，但笔者并未进行验证。对于一般的模型车而言，不需要改变默认配置。当相邻姿态距离和dt_ref的差超过正负dt_hysteresis时，规划器将改变这一距离。
-
global_plan_overwrite_orientation 根据官方文档，覆盖全局路径的方向是预留给3D路径规划使用的。对于车辆的2D规划，可以设置为False，可实现对全局路径的更好跟踪。
-
global_plan_viapoint_sep 从全局规划路径中提取的路点的相互距离。默认设置下为Disable，即只沿全局规划向前寻找，找到的离开局部规划器规划范围前的最后一个点作为局部目标。若全局规划特殊，在无障碍空间仍不走直线（例如，工作中的扫地机器人），则需要将此值设置为设置为一个小的正数。
-
max_global_plan_lookahead_dist 最大向前看距离。此距离1.应随车辆最大速度的增大而增大 2.不应超过激光雷达等传感器的可靠测量范围 3. 不应超过局部耗费地图的大小，即不能要求TEB对局部耗费地图以外的部分进行规划。
-
include_costmap_obstacles 必须设置为True后才能规避实时探测到的、建图时不存在的障碍物。
-
min_obstacle_dist 最小障碍物距离。若设置了车辆的轮廓，则不建议使用此参数。使用车辆轮廓配置footprint_model配合膨胀距离即可实现较好效果。障碍物惩罚权重很高，TEB不会违背这一最小距离约束，因此会把可以通过的缝隙视作不能通过，并终止运行。请注意，此处设置的机器人轮廓必须与局部耗费地图中的一致，后文将继续讨论。
-
inflation_dist 障碍物膨胀距离。此膨胀只是降低通过这些区域的优先级，不应当用此距离迫使车辆远离障碍物。
惩罚权重配置
在TEB规划器中，除了撞击障碍物不被允许外，其余的约束在没有可行方案时可被打破（如规划出事实上不可行的转弯半径）。对于权重配置，向读者介绍以下几种策略：
- 提高penalty_epsilon。此项为速度等约束提供类似膨胀层的缓冲效果。接近限速将产生一定的惩罚值。
大幅提高weight_optimaltime（最优时间权重）。提高至3-5时，车辆在直道上快速加速，并靠近路径边缘沿切线过弯。 - 大幅降低weight_optimaltime，设置较大的inflation_dist，并稍微增加weight_inflation。时间权重降低至0.3以下时，车辆速度明显平稳，不会向最高速度加速。膨胀距离大时将远离障碍物行驶。
大幅增加weight_kinematics_forward_drive（>100），禁止规划倒车。如仍规划倒车，说明找不到可行的前向行驶路径。若路径确实存在，可以检查转弯半径、各种footprint是否正确，膨胀层是否合理。
TEB中的恢复措施
恢复措施可以尝试将卡在杂物中的机器人或路径规划错误的机器人恢复至正常状态。TEB Local Planner实现了由move_base规定的振荡恢复方法。但是，实测中控制器将高频率振荡速度指令（>10Hz），读者应当评估自己的电动机能否承受。（可在 move_base配置中关闭）
TEB提供路径规划不可行（plan not feasible）时的恢复措施，称为shrink horizon backup。此时TEB将以更近的点作为规划目标，尝试重新规划出可行路径。调试时可关闭，以在可视化界面上观察原出错路径。
Costmap Converter
TEB默认情况下不使用Costmap Converter。事实上，此插件可以在复杂场景下极大提高运算效率，尤其是处理激光雷达分散的测量数据时。因为将障碍物视为系列孤立点效率极低。配置方法官方文档已经提供，此处不再赘述
Local Costmap 配置指导
Costmap的一般配置方法请参考相应官方文档。本文着重介绍为TEB规划器配置时的注意事项。
footprint配置：配置车辆/机器人的多边形外形。此外形应与TEB中的外形一致，或者至少在大小上没有太大的差别。注意costmap中的footprint设置是有默认值的，因此读者必须手动设置此配置为合适的值。配置举例：
local_costmap:
footprint: [[-0.305, -0.18], [-0.305, 0.18], [0.305, 0.18], [0.305, -0.18]]
costmap layers: 局部耗费地图需要有来自地图的静态层和来自传感器的动态层，缺一不可。笔者在最初尝试配置时忽略了静态层，此时TEB将无视全局地图中的障碍。许多传感器，例如2D激光雷达，不可能探测到实时障碍物（地图中没有的）背后的静态障碍物（地图中有的），TEB规划器可能规划出绕过前方障碍物，并穿过当前没有见到的障碍物的错误路径。速度较快时可能来不及在发现障碍物时改正。鉴于TEB已有障碍物膨胀功能，此处可以不再添加膨胀层。
局部耗费地图的更新频率应当不低于规划器和costmap_conveter的转换频率，否则显然是没有意义的。同时，局部耗费地图应当开启滑动窗口（rolling window）模式，窗口大小也不应大于传感器的探测距离。
关于多路径并行规划(Parallel Planning in distinctive Topologies)
前文已经指出，此功能将严重影响规划器性能。当路径中存在大量不连续分布的障碍物（如锥桶）并产生大量可行路径时，此功能有良好的效果。相反，若在单一路径上运行，则没有必要时用此功能。具体配置笔者并未做更多研究，还留待读者自行探索。
TEB规划器的性能问题
在本文的最后一部分，将总结一些可以提高TEB规划器性能的参数调整策略，供遇到与我类似问题的读者参考。
- 关闭多路径并行规划（效果非常显著）
- 使用Costmap Converter （非常显著）
- 降低迭代次数（no_inner/outer_iterations） （显著）
- 降低 max_lookahead_distance （一般）
- 减小局部耗费地图的大小 （显著）
- 增大规划周期和控制周期 （影响效果）
- 使用单点footprint，配合最小障碍物距离约束 （不太显著且影响效果）