原文

综述

TEB算法是局部寻路算法、全局寻路算法提供一个结果B,然后经过局部寻路算法进行细化为B^*再传递给机器人.TEB就是进行局部细化的工作.

TEB算法的核心思想.

算法的输入:B:=(Q,\tau).其中Q是全局寻路算法提供的若干个点状态(状态是一个三元式x,y,\beta.位置和偏向方向)的集合.\tau是每个状态的间隔时间.

算法的输出:给定一个优化后的B^*<em>,使得代价函数(或者叫目标函数)的值最低.

    \[B^{</em>*} = argmin \ f(B)\]

代价函数

我们可以简单地把目标函数设置成:

    \[f(B) = \sum_k \lambda_kf_k(B)\]


这个的意思就是代价函数可以设置成若干个项目的加权和.

项目可以分成两种:

  • 速度和加速度限制等约束
  • 轨迹有关的目标,如路径最短、路径最快或与障碍物的距离

我们一般使用下面的惩罚函数:

    \[e_{\Gamma}(x,x_j,\epsilon,S,n)=(\dfrac{x-(x_r-\epsilon)}{S})^n\ (x>x_r-\epsilon)\]


其中,x_r为极限值,Sn\epsilon影响近似的准确度。具体地说,S表示缩放,n表示多项式阶数,\epsilon表示近似值附近一个小位移。x就是输入,这里x和x_j是距离.

目标点和障碍点

假设一个点A,当前小车的与这个点的距离设置为\alpha = |\bold x|.

A是目标点.那么这个函数就是:

    \[f_{path} = e_{\Gamma}(\alpha,r_{pmax},\epsilon,S,n)\]


这代表,我们需要让这个小车离这个目标点越来越近.r_{pmax}是自己设置的临界值

A是障碍点.那么这个函数就是:

    \[f_{op} = e_{\Gamma}(-\alpha,-r_{omin},\epsilon,S,n)\]


这代表,我们需要让这个小车离这个目标点越来越远,所以说要加上负数.r_{omix}是自己设置的临界值.

速度和加速度

速度定义:

    \[v_i = \dfrac{1}{\Delta T_i}(x_{i+1}-x_i)\]


角速度定义:

    \[\omega_i = \dfrac{1}{\Delta T_i}(\beta_{i+1}-\beta_i)\]


加速度定义:

    \[a_i = \dfrac{2(v_{i+1}-v_i)}{\Delta T_i+\Delta T_{i+1}}\]

速度修正

当然我们在大学物理中学过,我们在考虑总体的速度的时候还要考虑转动所带来的影响.在此我们可以进行修正:

    \[v_{\omega_{R,j}} = v_i + L\omega_{i}\]

    \[v_{\omega_{L,j}} = v_i - L\omega_{i}\]

对上面的式子求差分我们可以得到加速度.

运动学约束

我们知道,机器人在运行的时候,路径不完全是由若干个直线组成的,而是由若干个圆弧组成的.我们取两个点i和i+1.两个点之间的连线设置为l,运动速度的方向设置为\beta_{i+1}\beta_i.两个方向和l的夹角\mathcal{Z}_{i+1}\mathcal{Z}_{i}一样,根据论文可以列出式子:

最快时间约束

就是每一段时间的平方和,如图所示:

    \[f_k = \sum (\Delta T_i)^2\]

实现

在初始化阶段,通过添加关于动力学和运动学约束的默认时间信息,将初始路径变为初始轨迹. 初始轨迹是由带有纯旋转和平移的分段线性分段组成的.这种以多边形表示的路径通常由概率路线图规划器提供.

L. E. Kavraki et al., ”Probabilistic roadmaps for path planning in high dimensional confifiguration spaces”, in IEEE Transactions on Robotics and Automation, Vol. 12, No.4, pp.566-580, August 1996.

在每次迭代中,算法动态地添加新的“configurations”或删除先前的“configurations”,以便将空间和时间分辨率调整到剩余的轨迹长度或规划的范围内.采用“hysteresis”来避免振荡。优化问题被转换为“hyper-graph”,并使用“ g2o”中包含的稀疏系统大规模优化算法进行求解.这一个部分的细节我们在下一个部分介绍

分类: 杂项

0 条评论

发表评论

Avatar placeholder

您的电子邮箱地址不会被公开。 必填项已用*标注

隐藏