1 TEB算法原理
TEB全程为Time Elastic Band (时间弹力带)，通过对给定的全局轨迹进行修正，从而优化机器人的局部运动轨迹。他是常用的局部路径规划方法之一。TEB是基于图优化的方法，以g2o优化框架实现，它以机器人在各个离散时间的位姿和离散时刻之间的时间间隔为顶点，通过多目标优化，包括但不限于：整体路径长度，轨迹运行时间，与障碍物的距离，通过中间路径点，以及机器人本身的动力学、运动学和几何约束的符合性。实现在各项约束下绕过障碍物最快到达目标点。
TEB源码：https://github.com/rst-tu-dortmund/teb_local_planner
TEB概念理解：https://www.leiphone.com/category/transportation/0TCtaBOIcFOIBN69.html
TEB参数理解：https://blog.csdn.net/weixin_44917390/article/details/107568507
- 列表项其中，起始点与目标点由全局规划器指定，中间插入N个控制橡皮筋形状的控制点（robot’s pose）
- 列表项在点与点之间定义运动时间Time。（点与点之间不是等距，而是等时间的）
- 这个路径可以变形，变形的条件就是将所有约束当作 一个橡皮筋 的外力
- 注意，每个目标函数只与elastic band的某几个连续状态有关，而非整条band
2.TEB 各项约束条件
定义机器人位姿：
其中$x_i$,$y_i$,$\beta_i$和分别对应机器人在map坐标系（或者世界坐标系）的位置和姿态，文章中称其为configuration。
则空间内configuration序列（如figure 1）记录如下：
跟随路径和避障约束
约束主要有两个目标：跟随已知的全局规划路径和避障。两个目标函数均十分相似，跟随路径施力将elastic bands拉向全局路径，而避障约束施力使得elastic bands远离障碍物。这种方法平衡了路径规划中的靠近与远离行为，确保了既定路径的遵循同时绕过障碍。
configuration序列与全局路径点序列或障碍物$z_{j}$的最近距离记为$d_{min,j}$，如figure 3所示。跟随路径目标以configuration距全局路径的允许最大距离$r_{p_{max}}$作为约束，避障目标以configuration距障碍物的允许最小距离$r_{o_{min}}$作为约束
速度/加速度约束
通过定义机器人姿态（robot pose）及相邻姿态间的时间差，利用差分法近似计算速度和加速度。这种方法允许对机器人的动态特性进行约束，确保其运动符合实际物理限制
运动学约束
实现平滑路径：通过组合若干弧线段形成平滑轨迹，避免车辆漂移现象。这通常通过控制车辆的速度和转向角来实现，如在ROS系统中，可以通过发布cmd_vel主题来控制Twist消息，从而精确调节车辆的运动。
差动机器人在平面运动只有两个自由度，其只能以朝向的方向直线运动或旋转。这种运动学约束使得机器人以有若干弧段组成的平滑的轨迹运动。相邻的两个configuration应在弧段的两端
最快路径约束
路径效率：目标函数旨在找到最快的路径，确保路径上的各个位置点在时间上均匀分布。重要的是，最快路径不总是最短路径，因为它还考虑了避障、速度和运动学约束。
目标函数即为最小化时间间隔序列的二次方。
TEB各项参数调节
#轨道配置参数
teb_autosize: True #优化期间允许改变轨迹的时域长度
dt_ref: 0.45 #局部路径规划的解析度# minimum 0.01
dt_hysteresis: 0.1 #允许改变的时域解析度的浮动范围， 一般为 dt_ref 的 10% 左右 minimum0.002
global_plan_overwrite_orientation: False #覆盖全局路径中局部路径点的朝向
max_global_plan_lookahead_dist: 3.0 #考虑优化的全局计划子集的最大长度
feasibility_check_no_poses: 5 #检测位姿可到达的时间间隔 minimum 0
de_ref指的是两个pose之间的时间间隔，其他参数通常直接用默认即可,de_ref根据具体需求进行调参。de_ref设置的越小，会产生更多的位姿点，从而使路径更加精细和平滑。但位姿点的增加也会进一步导致所需要的计算量的增加，可能会导致计算资源的消耗和处理时间的延长。此外，de_ref太小也会导致过度优化，降低鲁棒性等。
#机器人参数
max_vel_x: 0.5 #最大x前向速度，单位：m/s
max_vel_y: 0.3 #最大y前向速度，单位：m/s，非全向移动小车需要设置为0
max_vel_x_backwards: 0.5 #最大后退速度，单位：m/s
max_vel_theta: 1.5 #最大转向角速度，单位：rad/s
acc_lim_x: 0.2 #最大x向加速度，单位：m/s^2
acc_lim_y: 0.2 #最大y向加速度，，单位：m/s^2,非全向移动小车需要设置为0
acc_lim_theta: 0.3 #最大角加速度，单位：rad/s^2
#阿克曼小车参数，非阿克曼小车设置为0
min_turning_radius: 0.0 #机器人最小转弯半径
wheelbase: 0.0 #机器人轴距，前轮与后轮的距离
cmd_angle_instead_rotvel: False #true则cmd_vel/angular/z内的数据是舵机角度
#无论是不是阿克曼小车都设置为false，因为我们的阿克曼机器人内部进行了速度转换
#用于局部路径规划的机器人外形
#机器人外形的类型可以为：point、circular、two_circles、line、polygon，默认为point类型
footprint_model:
type: polygon
vertices: [[-0.133, -0.125],[-0.133, 0.125],[0.133,0.125],[0.133, -0.125]] #多边形端点坐标 for mini_mec
#目标点误差允许值
xy_goal_tolerance: 0.2 #机器人到达目标点时附近时的弧度偏差允许量，在该偏差内认为已经到达目标点，单位为：m
yaw_goal_tolerance: 0.1 #机器人到达目标点时附近时的弧度偏差允许量，在该偏差内认为已经到达目标点单位为：rad
free_goal_vel: False #允许机器人以最大速度驶向目的地
容忍度同样不建议设置的过小，容忍度过小会导致小车在目标点不断震荡。
free_goal_vel如果为Ture，小车则会在能加速的地方一直加速，当到达终点时，可能速度并不为0。
#障碍物参数
min_obstacle_dist: 0.1 #和障碍物最小距离，直接影响机器人避障效果
include_costmap_obstacles: True #是否将动态障碍物预测为速度模型，
costmap_obstacles_behind_robot_dist: 1.5 #限制机器人后方规划时考虑的局部成本地图障碍物
obstacle_poses_affected: 15 #障碍物姿态受影响0~30
costmap_converter_plugin: ""
costmap_converter_spin_thread: True
costmap_converter_rate: 5
include_dynamic_obstacles: True
dynamic_obstacle_inflation_dist: 0.6
include_costmap_obstacles这个参数通常设置为真（只有这个参数设置为真，才能规避实时探测到的建图时不存在的地图）。至于其他参数，通常保持默认设置即可。