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),将计算控制量和直接用于机器人系统。