TEB算法总结
1 TEB算法概述关于eletic band（橡皮筋）的定义：连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。
关于time eletic band的简述：起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态）；为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time，即为Timed-Elastic-Band算法。
注意：
[*]机器人的姿态指的是机器人坐标和朝向，称之为configuration;引入Time的概念，保证机器人运动是实时的，在一定时间内完成。
[*]Time Elastic Band算法把路径规划问题描述为一个多目标优化问题，即对最小化轨迹执行时间、与障碍物保持一定距离并遵守运动动力学约束等目标进行优化。因为优化的大多数目标都是局部的，只与机器人的某几个连续的状态有关，所以该优化问题为对稀疏模型的优化。
[*]求解稀疏模型多目标优化问题，可通过构建超图（hyper-graph），使用g2o（通用图优化）框架中关于大规模稀疏矩阵的优化算法来求解。机器人状态和时间间隔作为nodes，目标函数和约束函数作为edges，各nodes由edges连接构成hyper-graph。在该hyper-graph中，每个约束为一条edge，且每条edge允许连接的nodes的数目不受限制。
[*]Time Elastic Band算法通俗的解释：从给定路径中得到一系列带时间信息的离散位姿（pose），通过图优化的方法将这些离散位姿组成满足时间最短、距离最短和远离障碍物等目标的轨迹，同时满足机器人运动动力学的约束。需要注意的是，优化得到的轨迹并不一定满足所有约束，即给定的约束条件实际上都是软约束条件。
优点：
[*]适用于各种常见车模：如差分、全向、阿克曼模型
[*]有很强的前瞻性： 对前方一段轨迹进行优化
[*]动态避障效果好： 对动态障碍有较好的避障效果，可直接使用其封装好障碍类Obstacle
缺点：
[*]计算复杂度较大：可通过牺牲预测距离来降低复杂度
[*]速度和角度波动较大、控制不稳定： 源码中是通过两状态之间的距离和角度差及时间差来计算该控制周期内的速度和角速度，使得在控制过程中速度和角度波动较大。
[*]非全局最优： 但是优于DWA
2 Time Elastic Band模型
想要弄清TEB算法的原理以及整个流程，首先弄清楚插入的N各控制点是什么？Time是什么？通过TEB算法处理后最终得到什么？
经典的“elastic band”使用n个机器人的中间位姿
的序列来描述。其中 x_{i} 、 y_{i} 、 \beta_{i} 分别对应机器人在map坐标系（或者世界坐标系）的位置和姿态，文章中称其为configuration。
两个configuration间的时间间隔定义为 \Delta T_{i} ，记录时间序列：
将configuration及时间序列合并：
通过加权多目标优化获取最优的路径点，即最优的Q：
其中B*表示优化后的TEB， f(B) 表示目标函数。在本文中，它是包含各个方面的分量 f_{k} (B) 的加权和。
作者在撰写论文时ROS对稀疏矩阵优化问题的支持并未十分完善，故作者采用分段连续，可微分的代价函数计算破坏约束的惩罚值。
其中为 x_{r} 极限值， \epsilon 、S、n影响近似的准确度。具体地说，S表示缩放，n表示多项式阶数， \epsilon 表示限界值附近一个小位移。
通过上述的介绍，可以知道加入的N个控制点就是机器人的多个configuration，Time就是configuration之间的时间间隔，通过TEB算法的处理，最终得到一些列满足各种限制条件（速度限制、加速度限制、远离障碍物，符合全局路径等）的configuration和Time的集合。
3 约束目标函数
关于各个约束条件，分别阐述如下，在理清楚各个约束条件后，就可以通过优化的方法得到configuration和Time的集合。
3.1 Way points and obstacles
TEB在进行路标点跟踪的同时，还要避免撞到静态或动态障碍物。这两个目标函数的不同之处在于Way points吸引“elastic band”而障碍物排斥“elastic band”。目标函数取决于“timed elastic band”和Way points或障碍物之间的 z_{j} 之间的最小距离 d_{min,j} ，如下图所示。对于Way points这种情况，距离从上界以最大目标半径 r_{p_{max}} 约束；对于障碍物这种情况，距离从下界以最小距离 r_{o_{min}} 约束。
约束以惩罚函数实现：
值得注意的是，目标函数的梯度可视为对elastic band施加的外力。
3.2 速度和加速度
由速度和加速度组成的动力学约束可以用3.1中运动学约束的惩罚函数表示。机器人运动的平均线速度和角速度可以通过相邻的configuration： x_{i} x_{i+1} 和时间间隔 \Delta T 计算得到。
类似的，线速度约束可以表示为：
同理，角速度约束也可以得到。
3.3 运动学约束
差动机器人在平面运动只有两个自由度，其只能以朝向的方向直线运动或旋转。这种运动学约束使得机器人以有若干弧段组成的平滑的轨迹运动。相邻的两个configuration应在弧段的两端，如下图所示：
初始configuration x_{i} 与运动方向 d_{i,i+1} 的夹角 \theta_{i} 与下一状态的 \theta_{i+1} 相等。若 \beta_{i} 为机器人在第i段弧段相对于世界坐标系的绝对姿态，则有：
其中，运动方向向量为：
相应的目标函数：
3.4 最快路径约束
目标函数即为最小化时间间隔序列的二次方。
目标函数使得机器人获得最快路径，路径上的各configuration点在时间上均匀分开，而非传统的空间上求最短路径。
4 优化问题
我们理清configuration、Time等概念，并且理清楚各种约束条件后，我们通过构建hyper-graph进行求解。
使用开源框架g2o进行优化：点（node）&边（edge）;
g2o:General Graph Optimization通用图优化法
TEB算法就是求解configuration和Time和集合问题，也就是多目标优化问题，可通过构建超图（hyper-graph），使用g2o（通用图优化）框架中关于大规模稀疏矩阵的优化算法来求解。机器人状态和时间间隔作为nodes，目标函数和约束函数作为edges，各nodes由edges连接构成hyper-graph。在该hyper-graph中，每个约束为一条edge，且每条edge允许连接的nodes的数目不受限制。TEB hyper-graph如下图所示：
5 TEB算法实现流程
全局路径——>加入约束——>g2o优化——>速度指令
参考文献
《Trajectory modification considering dynamic constraints of autonomous robots》
https://junjun.blog.csdn.net/article/details/112155697?spm=1001.2014.3001.5506
https://blog.csdn.net/xiekaikaibing/article/details/83417223?spm=1001.2014.3001.5506
https://zhuanlan.zhihu.com/p/391093849
https://blog.csdn.net/Draonly/article/details/124563409?spm=1001.2014.3001.5506
页:
[1]