**局部路径规划-teb算法**

局部路径规划-teb算法

timed-elastic-band (定时间点 橡皮筋)
总体来说TEB算法就是，在全局路径上以固定的时间间隔插入n个姿态点，让路径变成一个可以变形的橡皮筋，然后再给它施加一些约束，每一种约束都可看做是障碍物或外力，给橡皮筋施加力以后，橡皮筋就会变形（即其内部的优化算法），通过变形就可以找到最终可行的路径。

导航功能包中，局部路径规划器的位置在local_planner，输出的是控制信息

介绍常见的4种约束外力
1）跟随路径和避障。
跟随路径是让小车紧贴全局路径，避障是让小车远离障碍物。前是拉近，后是推开，属于同一类问题。
2）速度 / 加速度约束
我们的橡皮筋只定义了姿态（x,y,θ）与两两状态之间的时间，所以就直接用差分近似计算好了。
由于小车的性能约束，v / a都有最大最小值。
3）运动学限制
若干弧段组成的轨迹，平滑，不希望出现漂移；我们的控制量只有车速（油门）与转角（方向盘）。
（阿克曼结构有最小转弯半径，麦轮/差速/全向轮车为0）

4）最快路径约束
目标函数使得机器人获得最快路径，路径上的各位姿点在时间上均匀分开，而非传统的空间上求最短路径。

优化
局部优化：“TEB”被表述为一个多目标优化问题，大多数目标都是局部的，只有一小部分参数相关，因为他们只依赖于几个连续的机器人状态。(所以优化的速度是非常快的)
优化算法：使用开源框架“g2o”：点（node）&边（edge）
g2o: General Graph Optimization通用图优化法。

TEB生成的局部轨迹由一系列带有时间信息的离散位姿（pose）组成，g2o算法优化的目标即这些离散的位姿，使最终由这些离散位姿组成的轨迹能达到时间最短、距离最短、远离障碍物等目标，同时限制速度和加速度，使轨迹满足机器人的运动学。

简洁描述：首先获取全局路径，等时间间隔的插入n个状态点，把全局路径变成一条橡皮筋；然后加入约束条件，给橡皮筋施加外力，再通过g2o进行轨迹优化，最后生成速度指令。