参考以下链接
http://wiki.ros.org/teb_local_planner
http://wiki.ros.org/teb_local_planner/Tutorials/Obstacle%20Avoidance%20and%20Robot%20Footprint%20Model
http://wiki.ros.org/teb_local_planner/Tutorials/Configure%20and%20run%20Robot%20Navigation
http://wiki.ros.org/action/fullsearch/teb_local_planner/Tutorials/Planning%20for%20car-like%20robots?action=fullsearch&context=180&value=linkto%3A%22teb_local_planner%2FTutorials%2FPlanning+for+car-like+robots%22
https://blog.csdn.net/weixin_44917390/article/details/107568507?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166977853616782390571244%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166977853616782390571244&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-6-107568507-null-null.142^v67^wechat_v2,201^v3^control_2,213^v2^t3_control1&utm_term=teb%20local&spm=1018.2226.3001.4187
https://blog.csdn.net/weixin_41349117/article/details/100636681?spm=1001.2101.3001.6661.1&utm_medium=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-100636681-blog-107568507.pc_relevant_default&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-100636681-blog-107568507.pc_relevant_default&utm_relevant_index=1
https://blog.csdn.net/zz123456zzss/article/details/104692548?ops_request_misc=&request_id=&biz_id=102&utm_term=%20TebLocalPlannerROS:%20trajector&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-7-104692548.142^v67^wechat_v2,201^v3^control_2,213^v2^t3_control1&spm=1018.2226.3001.4187
TEB算法简介
“TEB”全称Time Elastic Band（时间弹性带）Local Planner，该方法针对全局路径规划器生成的初始轨迹进行后续修正(modification)，从而优化机器人的运动轨迹，属于局部路径规划。在轨迹优化过程中，该算法拥有多种优化目标，包括但不限于：整体路径长度、轨迹运行时间、与障碍物的距离、通过中间路径点以及机器人动力学、运动学以及几何约束的符合性。“TEB方法”明确考虑了运动状态下时空方面的动态约束，如机器人的速度和加速度是有限制的。”TEB”被表述为一个多目标优化问题，大多数目标都是局部的，只与一小部分参数相关，因为它们只依赖于几个连续的机器人状态。这种局部结构产生了一个稀疏的系统矩阵，使得它可以使用快速高效的优化技术，例如使用开源框架“g2o”来解决“TEB”问题。详细理解参考qqfly文章qqfly
TEB规划器的性能问题的总结参考
- 关闭多路径并行规划（效果非常显著）
- 使用Costmap Converter （非常显著）
- 降低迭代次数（no_inner/outer_iterations） （显著）
- 降低 max_lookahead_distance （一般）
- 减小局部耗费地图的大小 （显著）
- 增大规划周期和控制周期 （影响效果）
- 使用单点footprint，配合最小障碍物距离约束 （不太显著且影响效果）
参数解析
ros中wiki有详细解释
http://wiki.ros.org/teb_local_planner#Parameters
也可参考参数
teb参数配置文件
TebLocalPlannerROS:
odom_topic: odom
map_frame: odom
#Trajectory
teb_autosize: True
dt_ref:0.45
dt_hysteresis:0.1
global_plan_overwrite_orientation: True
max_global_plan_lookahead_dist:3.0
feasibility_check_no_poses:3#Robot
max_vel_x:0.5
max_vel_y:0.5#max_vel_x:0.7
max_vel_x_backwards:0.2
max_vel_theta:0.4#max_vel_theta:1.5
acc_lim_x:1
acc_lim_y:1 #0.25
acc_lim_theta:0.80
min_turning_radius:0.0
footprint_model: # types:"point","circular","two_circles","line","polygon"#radius:0.12 # for type "circular"
type:"polygon"
vertices:[[-0.18,-0.11],[-0.18,0.11],[0.18,0.11],[0.18,-0.11]]#GoalTolerance
xy_goal_tolerance:0.2
yaw_goal_tolerance:0.5
free_goal_vel: False
#cmd_angle_instead_rotvel: True#Obstacles
min_obstacle_dist:0.25
inflation_dist:0.6
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist:1.0
obstacle_poses_affected:7
costmap_converter_plugin:""
costmap_converter_spin_thread: True
costmap_converter_rate:5#Optimization
no_inner_iterations:5
no_outer_iterations:4
optimization_activate: True
optimization_verbose: False
penalty_epsilon:0.1
weight_max_vel_x:1
weight_max_vel_y:1
weight_max_vel_theta:1
weight_acc_lim_x:1
weight_acc_lim_y:1
weight_acc_lim_theta:1
weight_kinematics_nh:1
weight_kinematics_forward_drive:1000
weight_kinematics_turning_radius:1
weight_optimaltime:1
weight_obstacle:50
weight_dynamic_obstacle:10 # not in use yet
selection_alternative_time_cost: False # not in use yet
#Homotopy Class Planner
enable_homotopy_class_planning: False
enable_multithreading: True
simple_exploration: False
max_number_classes:4
roadmap_graph_no_samples:15
roadmap_graph_area_width:5
h_signature_prescaler:0.5
h_signature_threshold:0.1
obstacle_keypoint_offset:0.1
obstacle_heading_threshold:0.45
visualize_hc_graph: False
#Recovery
shrink_horizon_backup: True
shrink_horizon_min_duration:10
oscillation_recovery: False
oscillation_v_eps:0.1
oscillation_omega_eps:0.1
oscillation_recovery_min_duration:10
oscillation_filter_duration:10
*Trajectory 参数调整轨迹*参考链接
dt_ref 局部路径规划的解析度（默认0.3）
dt_hysteresis 允许改变的时域解析度的浮动范围， 一般为 dt_ref 的 10% 左右（默认0.1）
dt_ref和dt_hysteresis 最优路径上的两个相邻姿态（即位置、速度、航向信息，可通过TEB可视化在rivz中看到）的默认距离
此距离不固定，规划器自动根据速度大小调整这一距离，速度越大，相邻距离自然越大。笔者认为，将dt_ref设置为与车辆长度大致是可行的。较小的值理论上可提供更高精度，但笔者并未进行验证。对于一般的模型车而言，不需要改变默认配置。当相邻姿态距离和dt_ref的差超过正负dt_hysteresis时，规划器将改变这一距离。
global_plan_overwrite_orientation 覆盖全局路径中局部路径点的朝向，有些全局规划者在开始和全局目标之间没有考虑局部子目标的方向，因此自动确定
覆盖全局路径的方向是预留给3D路径规划使用的。对于车辆的2D规划，可以设置为False，可实现对全局路径的更好跟踪
max_global_plan_lookahead_dist 考虑优化的全局计划子集的最大长度（累积欧几里得距离）（如果为0或负数：禁用；长度也受本地Costmap大小的限制），默认3.0
该参数决定局部规划初始轨迹的最大长度，实际调试发现此参数无需过大，因为局部轨迹在每个控制周期都被更新，实际执行的指令仅是轨迹上第一个点的速度值，这里设置为1.5（默认3.0）即可，过长也可能导致优化结果无法有效收敛。
此距离1.应随车辆最大速度的增大而增大 2.不应超过激光雷达等传感器的可靠测量范围 3. 不应超过局部耗费地图的大小，即不能要求TEB对局部耗费地图以外的部分进行规划。
feasibility_check_no_poses 检测位姿可到达的时间间隔
在判断生成的轨迹是否冲突时使用，此时设置为3，即从轨迹起点开始逐个检查轨迹上的3个点，若3个点均不发生碰撞，则认为本次轨迹有效，由于teb优化并非硬约束，这里相当于是轨迹生成之后的一层保障，这个参数因根据机器人的速度和环境复杂程度调整，否则极有可能出现在狭窄环境中走走停停的情况。
在 optimizer 返回一个轨迹后，并将速度指令发送给机器人之前，会运行一个可行性检查。目的是判定optimizer产生的无效/不可行的轨迹（软约束 ）。
现在，算法迭代从当前机器人pose开始的前 n 个 poses（n = ~/feasibility_check_no_poses），检查这些poses有无碰撞。为了检测碰撞的发生，使用了costmap footprint。 所以，该验证模型可能比之前优化的footprint更加复杂。
~/feasibility_check_no_poses不能太大，因为optimizer可能不会完全收敛：形象地说，small obstacle violations in the (far) future could be corrected while the robot is moving towards the goal。
这个参数根据机器人的速度和环境复杂程度调整，否则极有可能出现在狭窄环境中走走停停的情况，甚至可以减小到1或0
robot 参数根据实际配置机器人
ROS下的导航中local planner多数都针对两轮差速结构设计的，阿克曼转向结构由于结构的特点会有最小转向半径的限制，所以常见的local planner例如base_local_planner，dwa_local_planner等在这里都不适用里，这里使用的是teb_local_planner
它提供了一种car-like的动力学模型（即有最小转向半径限制），这样他所规划的路径也就符合了阿克曼结构的运动特点了
min_turning_radius 最小转弯半径（麦轮设为0）
为避免规划出不可能实现的移动路径，请动手测量实际车辆的转弯半径。此参数事实上约束了舵机的最大转角。有些车辆转向性能不佳，前轮实际转过角度小于舵机角度，则应当给指令转角乘上一增益后再控制舵机，否则车辆将总是不能实现设置的最小转弯半径。请注意此项应当与最大角速度配合设置
max_vel_x acc_lim_x x轴方向速度与加速度约束
max_vel_y acc_lim_y y轴方向速度与加速度约束
注意加速度同样约束减速过程。若电机性能良好且速度不快可以立即刹车，可直接将acc_lim_x设置为0，表示没有约束。若电机不能承受阶跃输入或者响应时间很长，则应当设置加速度限制
max_vel_x_backwards 最大倒车速度
将此速度设置为0或者负数将导致错误。禁止倒车应在penalty部分将前向行驶的权重设置得极高。
max_vel_theta acc_lim_theta 角速度和角加速度约束
正确配置下最小转弯半径应在低速时生效，而角速度限制应作用在车辆高速行驶时。角速度约束可以防止高重心转弯过快翻车，角加速度限制可以避免车轮打滑，车辆失控。对于低速平稳运行的车辆可以不约束此两项，将二者设为一个足够大的值即可。
footprint_model: # types: "point", "circular", "line", "two_circles", "polygon
机器人的 footprint 模型是为了达到优化目的而近似机器人的2D轮廓。它对计算距离的复杂度和时间至关重要，因此机器人 footprint 模型单独构建了一个参数，而没有直接调用 common costmap_2d 参数中的 footprint。优化 footprint 模型可能与costmap footprint 模型不同
1.Point
机器人被建模为一个点，这种类型所需的计算时间最少。
2.Circular
机器人被建模为一个简单的圆，半径为/footprint_model/radius。计算距离和Point类型相似，不同点是每次调用函数时机器人半径会被加入到参数min_obstacle_dist。可以通过选择Point类型，然后将半径加到最小障碍物距离来替换
3. Line
适用于长方形机器人，可以通过参数/footprint_model/line_start和/footprint_model/line_end来配置线段。机器人（旋转轴）被定在[0, 0]点，确保通过参数min_obstacle_dist来包含整个机器人（如下图）：
4.Two Circles
还可以通过两个圆来近似机器人轮廓。每个圆由机器人x轴的偏移量和半径来描述。/footprint_model/front_offset， /footprint_model/front_radius， /footprint_model/rear_offset， /footprint_model/rear_radius。Offsets 可能为负值。
5. Polygon
可以用多边形表示复杂模型，多边形由定点的列表组成。假设机器人旋转轴位于[0,0]（单位：米）。 请勿重复第一个顶点，因为多边形会自动闭合。
请记住，每个额外的边缘都会显着增加所需的计算时间！ 您可以从costmap公共参数文件中复制多边形 footprint 模型
GoalTolerance 参数设置机器人停止运行的容差
xy_goal_tolerance和yaw_goal_tolerance 目标跟踪误差
根据车辆运行精度设置。例如，笔者使用一台攀爬RC车构建导航系统，这种车辆转向间隙特性很大，则不应设置严格的航向限制
free_goal_vel 自由目标速度
设为False时，车辆到达终点时的目标速度为0。我们已经指出，TEB是时间最优规划器。缺少目标速度约束将导致车辆“全速冲线”，即使前方有一堵墙也是如此（因为撞墙的时刻不在规划器考虑范围内了）。不过此选项在竞速比赛中比较有用
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优化框架中（障碍物距离是目标函数之一，描述为超图的边）
TEB默认情况下不使用Costmap Converter。事实上，此插件可以在复杂场景下极大提高运算效率，尤其是处理激光雷达分散的测量数据时。因为将障碍物视为系列孤立点效率极低
include_costmap_obstacles 必须设置为True后才能规避实时探测到的、建图时不存在的障碍物。
min_obstacle_dist 最小障碍物距离
若设置了车辆的轮廓，则不建议使用此参数。使用车辆轮廓配置footprint_model配合膨胀距离即可实现较好效果。障碍物惩罚权重很高，TEB不会违背这一最小距离约束，因此会把可以通过的缝隙视作不能通过，并终止运行。请注意，此处设置的机器人轮廓必须与局部耗费地图中的一致。
增大min_obstacle_dist可以防止机器人离墙太近，但是对于狭窄通道的情况，反而应该设置的很小。 可以增大机器人轮廓，但减小min_obs_dist，提高防撞性，又保证过窄通道
min_obstacle_dist”参数限制机器人与障碍物的最小距离，实际还配合“obstacle_association_force_inclusion_factor”和“obstacle_association_cutoff_factor”这两个参数生效参考*TebOptimalPlanner::AddEdgesObstacles()*函数中的如下代码：
// iterate obstacles，obst变量只存储障碍物形式的一个单元，例如一个点，一条线，所以这里是迭代环境中的所有障碍物单元for(const ObstaclePtr &obst :*obstacles_){// we handle dynamic obstacles differently belowif(cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic())continue;// calculate distance to robot model//! 根据不同的机器人模型（点，圆，多边形等），不同的障碍物模型（点，线，多边形），有不同的距离计算方法double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get());// force considering obstacle if really close to the current poseif(dist < cfg_->obstacles.min_obstacle_dist * cfg_->obstacles.obstacle_association_force_inclusion_factor){
relevant_obstacles.push_back(obst.get());continue;}// cut-off distanceif(dist > cfg_->obstacles.min_obstacle_dist * cfg_->obstacles.obstacle_association_cutoff_factor)continue;// determine side (left or right) and assign obstacle if closer than the previous oneif(cross2d(pose_orient, obst->getCentroid())>0)// left{if(dist < left_min_dist){
left_min_dist = dist;
left_obstacle = obst.get();}}else{if(dist < right_min_dist){
right_min_dist = dist;
right_obstacle = obst.get();}}}
距离小于min_obstacle_dist * obstacle_association_force_inclusion_factor值的“障碍物点”，被强制加入优化框架中，距离大于min_obstacle_dist * obstacle_association_cutoff_factor的“障碍物点”被直接抛弃不再考虑，然后在剩余的障碍物点中计算机器人左侧最小距离和右侧最小距离。这三个参数的设置非常重要，需要根据机器人的外形尺寸小心调整，否则极易出现狭窄空间机器人无法通过或优化不收敛的情况。
inflation_dist 障碍物膨胀距离
默认0.0 障碍物膨胀距离。这个值必须大于min_obstacle_dist才有效。源码在AddEdgesObstacles. 此膨胀只是降低通过这些区域的优先级，不应当用此距离迫使车辆远离障碍物。障碍物周边的缓冲区，零惩罚代价，缓冲区会导致规划器减速
costmap_obstacles_behind_robot_dist double 限制在机器人后面规划时考虑到的占用的本地成本图障碍(指定距离，单位为米)
obstacle_poses_affected int 障碍物位置与轨迹上最接近的姿态相连，以减少计算量，但同时也考虑了许多相邻的障碍物
Optimization 参数主要是设置优化框架中各部分的权重大小参考链接
no_inner_iterations int 被外循环调用后内循环执行优化次数 15100
no_outer_iterations int 执行的外循环的优化次数 14100
optimization_activate bool 激活优化 False True True
optimization_verbose bool 打印优化过程详情 False False True
penalty_epsilon double 对于硬约束近似，在惩罚函数中添加安全范围 0.00.11.0
weight_max_vel_x double 最大x速度权重 0.02.01000.0
weight_max_vel_y double 最大y速度权重 0.02.01000.0
weight_max_vel_theta double 最大加速度权重 0.01.01000.0
weight_acc_lim_x double 最大x 加速度权重 0.01.01000.0
weight_acc_lim_y double 最大y 加速度权重 0.01.01000.0
weight_acc_lim_theta double 最大角速度权重 0.01.01000.0
weight_kinematics_nh double 满足非完整运动学的优化权值 0.01000.010000.0
weight_kinematics_forward_drive double 优化过程中，迫使机器人只选择前进方向，差速轮适用 0.01.01000.0
weight_kinematics_turning_radius double 优化过程中，车型机器人的最小转弯半径的权重 0.01.01000.0
weight_optimaltime double 优化过程中，基于轨迹的时间上的权重 0.01.01000.0
weight_obstacle double 优化过程中，和障碍物最小距离的权重 0.050.01000.0
no_inner_iterations:5 图优化optimizer的迭代次数
no_outer_iterations:4 每次外部循环迭代都会根据所需的时间分辨率dt_ref自动调整轨迹的大小，并调用内部优化器
penalty_epsilon 0.1 一次性改变所有的惩罚项，为惩罚函数增加一个小的安全余量，以实现硬约束近似. 例如为速度的约束提供一个缓冲的效果，在到达速度限制前会产生一定的惩罚，让其提前减速达到缓冲的效果
如果选择过高的值，会影响最终的信息矩阵，可能会出现不收敛的情况
weight_obstacle 默认值：50.0. 优化权重以保持与障碍物的最小距离。可以增大至几百，让机器人提前转向，避免卡死
obstacle_cost_exponent 无默认值，目前设置为0.65. 源码在EdgeObstacle::computeError和EdgeInflatedObstacle::computeError，判断是否为1，同时min_obstacle_dist >0才有效。用于更新g2o的误差函数
weight_kinematics_nh 默认值：1000. 用于满足非完整运动学的优化权重. 该参数必须很高，因为运动学方程构成了一个等式约束，即使值1000也不意味着由于与其他成本相比较小的“原始”成本值而导致的矩阵条件不佳
weight_kinematics_nh”参数应设置较小值，因为我们是完整约束机器人无需限制其运动学约束。
weight_kinematics_forward_drive 默认值：1，改为1000。
迫使机器人仅选择前进方向 (正向速度) 。较小的权重，仍然会允许向后行驶。我们无法限制优化器的搜索空间为正的速度，因为优化器不支持硬约束，只能大幅提高weight_kinematics_forward_drive以避免后退速度，但是值太大又会减小收敛速度。不管权重多高，无法规划出前进的路径时，还是可能出现停滞或者倒车。
weight_optimaltime 最优时间权重
提高至5时，车辆在直道上线速度明显加快，而且越大越能接近max_vel_x，并靠近路径边缘沿切线过弯。 Optimization weight for contracting the trajectory w.r.t transition time
weight_inflation 默认0.1， Optimization weight for the inflation penalty，应当很小，源码在AddEdgesObstacles的信息矩阵里
Homotopy Class Planner 参数与HomotopyClass规划器相关 并行规划
此功能将严重影响规划器性能。当路径中存在大量不连续分布的障碍物（如锥桶）并产生大量可行路径时，此功能有良好的效果。相反，若在单一路径上运行，则没有必要时用此功能。
当enable_homotopy_class_planning = False，规划的局部路径会陷入局部最小值。因为路径向上面障碍物或下面障碍物偏，总体的代价值都会增加（时间成本），只有往中间走总体代价值才最低。但此时路径invalid，中间缺少路径点，此时机器人卡死了。
设置True，teb会同时搜寻多条路径并选取一条更可行的，feasibility check环节会抛弃不可行的路径
- enable_multithreading (bool，默认：true) 激活多个线程以计划不同线程中的每个轨迹
- max_number_classes (int，默认值：4，设置为2) 指定要考虑的最大不同轨迹数(限制计算工作量)，影响CPU
- selection_cost_hysteresis (double，默认值：1.0) 指定新候选人必须具有多少轨迹成本才能选择之前的轨迹 (如果new_cost < old_cost * factor，则进行选择
- selection_prefer_initial_plan 默认0.95 Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan
- selection_obst_cost_scale (double，默认值：100.0) 仅选择“最佳”候选者的障碍物成本术语的额外缩放
- selection_viapoint_cost_scale (double，默认值：1.0) 仅为了选择“最佳”候选者而对通孔成本条款进行额外的缩放
- selection_alternative_time_cost (bool，默认值：false) 如果为true，则将时间成本 (时间差平方的总和) 替换为总过渡时间 (时间差之和)
- simple_exploration 默认false. 如果为true，不同的 trajectories are explored using a simple left-right approach (pass each obstacle on the left or right side) for path generation, otherwise sample possible roadmaps randomly in a specified region between start and goal.
- roadmap_graph_no_samples (int，默认值：15) 指定为创建roadmap graph而生成的样本数，前提是simple_exploration为false
- roadmap_graph_area_width (double，默认值：6) 在起点和目标之间的矩形区域中采样随机关键点/航路点。以米为单位指定该区域的宽度
- delete_detours_backwards 默认true. planner will discard the plans detouring backwards with respect to the best plan
- h_signature_prescaler (double，默认值：1.0) 标度内部参数 (H-signature) ，用于区分同伦类。警告：仅当您在本地成本图中观察到障碍物过多的问题时，才减小此参数，请勿将其选得太低，否则障碍物无法彼此区分 (0.2 <值 <= 1)
- h_signature_threshold (double，默认值：0.1) 如果实部和复杂部的差都低于指定的阈值，则假定两个H签名相等
- obstacle_heading_threshold (double，默认值：1.0) 在障碍物航向和目标航向之间指定标量乘积的值，以便将障碍物)考虑在内进行探索
- visualize_hc_graph (bool，默认：false) 如果为true，在rviz里可看到优化使用的graph
- viapoints_all_candidates (bool，默认：true) 如果为true，则将不同拓扑的所有轨迹附加到该组通孔点，否则，仅将与初始/全局计划共享相同拓扑的轨迹与它们连接 (对test_optim_node无效)
- switching_blocking_period (double，默认值：0.0) 指定允许切换到新的等效类之前需要终止的持续时间 (以秒为单位)
版权归原作者 Crush Mome 所有， 如有侵权，请联系我们删除。