当前位置:   article > 正文

teb局部路径规划算法

teb局部路径规划

TEB算法基本原理

DWA、TEB、Trajectory Rollout是三种常用的局部路径规划算法。其中,TEB算法有很强的前瞻性,能够动态避障,表现较好,缺点是计算复杂——这也是其使用g2o算法优化计算量的原因。

TEB全称是Timed Elastic Band,译为时间橡皮筋。好比导航的起点和终点之间,拉了一条绷直的橡皮筋作为行走路线,如果路中间有障碍物,就会将橡皮筋撑开,影响行走时的路径。

就像这样:
在这里插入图片描述
当然,在实际情况中,机器人的行走路线会受到许多其他因素的影响,不会紧贴着障碍物表面。我们通常会将障碍物进行膨胀处理,让机器人的行走路线与真实的障碍物之间,保持一定的安全距离。
在TEB算法中,所有影响因素分为两类:约束和轨迹。
约束指的是速度和加速度的限制,这些往往由机器人本身决定;轨迹则指最短或最快的路径,或者与障碍物保持距离,又或者是与规划路径尽可能地跟随。

一般来说,TEB算法会通过参数的方式,调整各影响因素的权重,为每个影响因素设计奖励或者惩罚函数,然后对可行的路线进行打分,最后选择得分最高的局部路线下发速度指令。

这样规划出的路径,更符合小车的硬件结构以及运动学,能够让小车流畅地行驶和绕障,且行驶速度和到点时间都有着优秀表现。

但由于需考虑的参数较多,如何「合理地」配置参数,就成了最重要且最困难的一环。

如何为TEB算法配置参数?

典型的「障碍物和中间点参数配置」

通常来说,我们会希望为机器人规划出一条远离障碍物,又尽可能经过中间点的路线,让机器人的行走路线更合理。基于此,我们可以在TEB算法中,对障碍物和中间点分别配置参数,并运用到惩罚函数中。
在这里插入图片描述TEB与障碍物的最小距离演示,Xn为TEB路径点,Zj为障碍物
一方面,为远离障碍物,我们需要为机器人设置障碍物距离、障碍物膨胀距离,以及障碍物的权重等障碍物参数,对靠近障碍物的路线进行惩罚。

另一方面,为尽可能经过路线中间点,我们可以以相反的目的进行参数设置,如中间点权重、中间点间隔和中间点顺序等,对远离中间点的路线进行惩罚。

具体的惩罚公式如下:
请添加图片描述(惩罚函数,d为TEB路径距离障碍物的实际最小距离,r为参数定义的最小距离)
由于障碍物和中间点相互独立,有时会出现障碍物阻挡在TEB路径,或中间点坐落在障碍物内部的情况。

这两种参数就像橡皮筋上的同一个点,受到相反方向两个力的牵制,一旦权衡控制不当,就会让机器人陷入局部最优困境,增大绕障时间,甚至让机器人撞向障碍物。
在这里插入图片描述局部最优困境演示

更复杂的「速度和加速度参数配置」

TEB算法还考虑到了机器人本身速度和加速度的限制,如对线速度、角速度、线加速度、角加速度各自的最大限制和到达最大值的权重进行参数配置。

首先,速度约束在TEB中能用固定时间间隔的特性解释。TEB路线中某一点的速度,可以由其前后点的距离推出:
请添加图片描述
请添加图片描述
同理,在得到速度之后,可以用同样的时间间隔推出加速度:
请添加图片描述在差分驱动机器人上,车轮的速度可以表示为:
请添加图片描述之后再进一步微分,就能得到车轮的加速度。最后,根据计算出的车轮速度与由最大速度制定的损失函数综合考虑,就能制定出固定时间下速度和位移合适的路线。

丝滑的「运动学约束」

有些机器人底盘还会受到运动学约束,如差分驱动机器人,它在平面运动中只有两个自由度,只能在机器人朝向上执行运动。因此还需要进行运动学约束,规划出一条相对平滑的路线,让机器人走得更丝滑。
TEB路径规划就考虑到了这点,同样以差分驱动机器人为例:
请添加图片描述
在这里插入图片描述

「最快路径轨迹约束」

最快路径轨迹约束,其实就是时间约束,需要对最优时间权重进行参数设置。即间接地对TEB路径点之间的时间间隔进行控制,从而影响最终到达时间。

如果TEB路径点之间的时间间隔是一致的,那么最终规划出的轨迹,就是约束后时间最短的路径轨迹。具体可以通过最小化所有时间间隔之和的平方来实现,这也是TEB算法最根本的特征和优势。
请添加图片描述
总的来说,我们在实践过程中,要根据TEB算法的规划原理,依次考虑障碍物和中间点、速度和加速度、运动学约束以及最快路径轨迹约束等参数配置,才能让机器人走出流畅又丝滑的步伐。

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/凡人多烦事01/article/detail/697904
推荐阅读
相关标签
  

闽ICP备14008679号