《Trajectory modification considering dynamic constraints of autonomous robots》理解

        eletic band（橡皮筋）的定义：连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。关于time eletic band的简述：起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time，即为Timed-Elastic-Band算法。

一、Timed Elastic Band概述

定义机器人位姿：，其中，和分别对应机器人在map坐标系（或者世界坐标系）的位置和姿态，文章中称其为configuration。

则空间内configuration序列（如figure 1）记录如下：

                                                         

 两个configuration间的时间间隔定义为：，表示机器人由一个configuration运动到另一个configuration所需时间；

记录时间序列：

将configuration及时间序列合并：

通过加权多目标优化获取最优的路径点，即最优的Q：

                                                                   

其中为最优结果，为考虑各种约束的目标函数，为个目标函数的权值。

在此应注意，每个目标函数只与elastic band中的某几个连续状态有关，而非整条band。故优化为对稀疏矩阵模型的优化。

二、约束目标函数

1、跟随路径和避障约束

约束主要有两个目标：跟随已知的全局规划路径和避障。两个目标函数均十分相似，跟随路径施力将elastic bands拉向全局路径，而避障约束施力使得elastic bands远离障碍物。configuration序列与全局路径点序列或障碍物的最近距离记为，如figure 4所示。跟随路径目标以configuration距全局路径的允许最大距离作为约束，避障目标以configuration距障碍物的允许最小距离作为约束。

                                 

约束以惩罚函数实现：

                                                   

 值得注意的是，目标函数的梯度可视为对elastic band施加的外力。

2、速度和加速度约束

由速度和加速度组成的动力学约束可以用类似1中运动学约束的惩罚函数表示。以figure 4为例，机器人运动的平均线速度和角速度可以通过相邻的configuration和时间间隔计算得到。

                                                               

类似的，线速度约束可以表示为：

                                                                 

 机器人的平均线加速度用同样的近似计算得到：

                                                                    

角加速度同理可计算得到。

3、最快路径约束

目标函数即为最小化时间间隔序列的二次方。

                                                                           

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

三、teb实现

teb的控制流程如下图：

                                              

teb优化问题（公式4）可以转化为hyper-graph问题。configurations和以及时间间隔作为nodes，目标函数以及其他约束函数为edges，各nodes由edges连接起来构成hyper-graph。该graph中，每一个约束都为一条edge，并且每条edge允许连接的nodes的数目是不受限制的。

（a）中展示了两个configurations(s0,s1)，一个时间间隔ΔT0，一个障碍点O1组成的hyper-graph。速度约束需要计算平均速度，则该约束与s0,s1,

ΔT0相关，该edge将三个nodes连接起来。障碍物约束将障碍物和离障碍物最近的configuration连接起来。注意障碍物点是固定的，算法不应也不能对其位置进行优化。

（b）中则展示了更大的teb hyper-graph。

                         

在确认能进行teb优化后（Verify trajectory），将计算控制量和直接用于机器人系统。