一、简介
“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（向前规划距离）调小，可以使得局部路径较贴近全局路径并完成跑图，再慢慢调大找最佳值
目前状况：
我把时间最优设置到最大的情况下，小车很喜欢走直线，但是不知道是不是建图的问题，也有小车本身的问题，他很容易被赛道杆子下面那一块卡住，直接导致雷达扫描出问题，路径规划有时候会出错，这个情况我初步认为是建图问题，建图有点歪，赛道附和到一起，导致规划异常。
但比较致命的问题是，小车总喜欢卡弯道，太贴合内弯，之前设置的静态障碍物最小距离和膨胀距离起的作用不大，我不太理解，然后再更换一下雷达，提高一下扫描的精度，接着就是转弯角的问题，不需要设置的太大，这个我们在调建图的过程慢慢寻找合适的转弯角度。
我认为现在主要的问题是需要把膨胀的范围拉大，让小车尽量不走小路，还有就是代价地图的设置，这个和上面的膨胀都是由关系的，所以我们首要就是要把这个设置好，现在的最短路径不需要太考虑，尽量远离障碍物。
可以尝试调搞附近的范围
把局部地图范围提高到4*4，
下面为common文件，是全局和局部代价地图所共有的参数
#---(in meters)---
footprint: [ [-0.035,-0.1], [0.18,-0.1], [0.18,0.1], [-0.035,0.1] ] #设置小车大小
#这个可以根据实际设计，但是我认为可以更大一点，给小车预留一些空间
transform_tolerance: 0.2
obstacle_layer:
enabled: true
obstacle_range: 2.5
raytrace_range: 3.0
inflation_radius: 0.05
track_unknown_space: false
combination_method: 1
observation_sources: laser_scan_sensor
laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}
inflation_layer:
enabled: true
cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10)
inflation_radius: 0.1 # max. distance from an obstacle at which costs are incurred for planning paths.
static_layer:
enabled: true
map_topic: "/map"
根据上面我们注意以下参数
inflation_radius: 0.05
这个是障碍物膨胀半径，应该设置为和障碍物之间的最小距离，让小车不要走小路，但是为了防止卡住小车的路径(障碍物在中间，膨胀过大导致和左右膨胀重合，导致无法过去)，太小了会导致走小路和拐弯出现问题，这个是我们需要注意的，所以目前设置为0.30
global_costmap需要遵循的配置
global_costmap_params.yaml
global_costmap:
global_frame: /map
robot_base_frame: base_link
update_frequency: 5.0
static_map: true
local_costmap需要遵循的配置
local_costmap_params.yaml
local_costmap:
global_frame: odom
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 6.0
height: 6.0
resolution: 0.05
接下来尝试修改：
inflation_radius: 0.30 在common文件下
max_vel_x: 0.7
max_vel_x_backwards: 0.2
max_vel_y: 0.0
max_vel_theta: 0.40 # 防止过大震荡
acc_lim_x: 0.2
acc_lim_theta: 0.2
min_turning_radius: 0.375 # 最小转弯角度，这个还需要慢慢调试
xy_goal_tolerance: 0.5
yaw_goal_tolerance: 0.4 # 位置和角度的容忍度开大没有关系
free_goal_vel: True # 可以自由冲刺
min_obstacle_dist: 0.30 # 与障碍物最小距离(整个线的头 中间 尾)
# 我好奇的是一个小问题，障碍物膨胀和地图本身难道是没有关系的吗，还是说这个是独立计算的，具体还需要再尝试
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: 1 # 膨胀区域权重 忘记最大是多少，这些避开障碍物稍微调大一点
weight_dynamic_obstacle: 10 # 动态障碍物最小距离权重
weight_dynamic_obstacle_inflation: 0.2 # 动态障碍物膨胀区域权重