一、简介
“TEB”全称Time Elastic Band（时间弹性带）Local Planner，该方法针对全局路径规划器生成的初始轨迹进行后续修正(modification)，从而优化机器人的运动轨迹，属于局部路径规划。
关于eletic band（橡皮筋）的定义：连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。
二、说明
局部路径规划之Teb
起始点、目标带你状态由全局规划，中间插入N个控制节点来改变橡皮筋的状态控制点，在点与点之间定义运动时间Time。
这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力
注意每个目标函数只与几个连续状态有关，而非整条band。
当我们设置目标时，小车的目的地是靠近障碍物的，但是如果我们小于我们的障碍物距离最小距离，我们就需要往外拉，这就是我们的类橡皮筋过程
约束目标函数：
优化问题：
Teb优化问题实质上是一个优化问题，大多数目标是基于局部的，只与一小部分参数相关，因为他们只依赖于几个连续的机器人
TEB生成的局部轨迹由一系列带有时间信息的离散位姿组成，g2o算法优化的目标就是这些离散的位姿，同时设计一条时间最短，距离最短，远离障碍物等目标，同时限制速度与加速度使轨迹满足机器人运动学。
整体的规划为：
全局路径————加入约束————g2o优化————速度指令
参数：
# Trajectory
teb_autosize: True #优化期间允许改变轨迹时域长度
dt_ref: 0.3 #局部路径规划解析度(0.01~1.0) 默认为0.3 (两个相邻位姿之间的时间_时间分辨率)
(可以设置0.45，其他保持默认)
dt_hysteresis: 0.1 #允许浮动范围
global_plan_overwrite_orientation: True #覆盖全局路 径中局部路点朝向
max_global_plan_lookahead_dist: 3.0 #全局优化子集最大长度
feasibility_check_no_poses: 2 # 检测位姿可达到的时间间隔
# Robot _ 下面的这些参数就会切实的影响规划
max_vel_x: 0.7 #最大前进速度 x (可以设置成0.5尝试)
max_vel_x_backwards: 0.3 #最大后退速度 x 这个值不能为0或者负数，否则会错误
(无法阻止倒车，就算比例很大，在迫不得已的情况下)
max_vel_y: 0.0 # y方向最大速度 阿克曼形是没有的
max_vel_theta: 0.50 #最大转向角速度
acc_lim_x: 0.15 # 最大加速度
acc_lim_theta: 0.20 #最大角加速度，不建议很大，会导致震荡
min_turning_radius: 0.35 #最小转弯半径
这个参数的设置是非常影响转弯时的路径规划的
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "line" # 设置机器人模型——可以为上面几种
line_start: [0.05, 0.0] # for type "line" #线的起点
line_end: [0.10, 0.0] # for type "line" #线的终点
(这种线的模式适用于阿克曼小车，设置线的起点和终点)
# GoalTolerance 目标容忍度
前两个比较重要，但不介意设置的很小
xy_goal_tolerance: 0.2 #xy目标偏移度
yaw_goal_tolerance: 0.2 #目标角度偏移容忍度
free_goal_vel: True #允许机器人以最大速度前往目的地，True的话就会在可以加速的过程中加速
complete_global_plan: True #完成目标点
# Obstacles 障碍物
min_obstacle_dist: 0.30 # 与障碍物最小距离(整个线的头 中间 尾)
这个参数的设置决定了和障碍物约束的距离
测试之后不建议设置的很小，因为我们的车模本身就是小的，而且地面有凸起，所以离得远点
inflation_dist: 0.30 # 障碍物膨胀距离
include_costmap_obstacles: True # 局部地图中的实时障碍物是否考虑
costmap_obstacles_behind_robot_dist: 0.3 #代价地图考虑后方的障碍物(因为是有倒车的)
obstacle_poses_affected: 7 #障碍物姿态影响，影响不是特别大
dynamic_obstacle_inflation_dist: 0.6 #动态障碍物膨胀范围
include_dynamic_obstacles: True #是否将为速度模型
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 # 对硬约束近似
这个参数会为速度约束，也就是达到最大速度前，会有个惩罚，会让他提前减速达到缓冲效果
obstacle_cost_exponent: 4
weight_max_vel_x: 2 # 最大速度权重
weight_max_vel_theta: 1 # 最大角速度权重
weight_acc_lim_x: 1 # 最大加速度权重
weight_acc_lim_theta: 1 # 最大角速度权重
(上面的参数主要起综合作用，确定是高速还是低速)
weight_kinematics_nh: 1000
weight_kinematics_forward_drive: 500 #抑制倒车的权重，正常设置1
weight_kinematics_turning_radius: 1 #最小转弯半径，我们没必要最小转弯
weight_optimaltime: 500 #优化时间参数，让小车多走直线和内道
weight_shortest_path: 0
weight_obstacle: 50 # 优化过程中和障碍物最小距离权重
weight_inflation: 0.2 # 膨胀区域权重
weight_dynamic_obstacle: 10 # 动态障碍物最小距离权重
weight_dynamic_obstacle_inflation: 0.2 # 动态障碍物膨胀区域权重
weight_viapoint: 1 #路径采样点距离权重
上面的这些参数是我们修改比较多，也是影响比较大的参数
注意点：
1.当小车在某些位置卡住不敢走（还没撞）时，考虑膨胀半径是否应该调小，和weight_optimaltime是否应该调大。
2.刚开始调，可以速度调慢点，max_global_plan_lookahead_dist（向前规划距离）调小，可以使得局部路径较贴近全局路径并完成跑图，再慢慢调大找最佳值
