TEB算法中机器人平稳避障策略研究

赵晓东，曹梦颖，宿景芳

(河北科技大学 信息科学与工程学院，河北 石家庄 050018)

0 引言

近年来,服务型移动机器人在自动化、计算机和人工智能等领域已经成为研究热点[1],在各类服务行业中的需求也比较迫切。能否对这些移动机器人开发普遍适用且高效安全的路径规划系统及能否具有自主感知能力等是移动机器人完成整个行程任务的热点和难点[2-3]。在简单已知环境下的路径导航一般使用全局路径规划算法,传统的全局路径规划算法有一般的启发式算法[4],如Dijkstra算法、A*算法和RRT算法等。而当移动机器人处于未知或部分信息的环境中时,则需要结合当前所遇到的局部环境信息进行实时路径调整[5],即需要用到局部路径规划算法。局部路径规划算法可以使移动机器人根据障碍物的位置和特征进行位置调整,从而在全局导航算法的指导下根据本地周围实时环境规划更优的行走路径。

动态社会环境中的局部移动机器人路径规划方法主要有无机器人动力学的方法和基于机器人动力学的方法,无机器人动力学的导航方法是指未考虑机器人的动力学和运动学,只根据周围环境和当前状态确定接下来的运动方向,比如人工势场法[6]、矢量场直方图法[7]、弹性带[8]和速度障碍法[9]等已被证明能够使机器人自主安全避开障碍物,并导航到给定目标,但在现实环境中直接利用输出控制命令来控制移动机器人比较困难。要想在实际社会环境中使机器人自主避障和寻路,需要考虑机器人动力学约束,即基于机器人动力学的方法。时间弹性带(Time Elastic Band,TEB)算法[10-11]是一种典型的基于机器人动力学的局部路径规划算法,国内外很多研究者对其进行了研究,是一种实用的机器人局部路径规划算法。TEB算法来源于弹性带(Elastic Band, EB)算法,EB算法将要规划的路径看作一根可以随意变形的弹性带,通过施加内部收缩力使路径最短,并通过从障碍物向外辐射斥力使路径变形避开障碍物,其目标是使移动机器人在尽量避开障碍物的情况下按最短路径行进,但EB算法没有考虑动力学约束,因此需要一个专用的路径跟踪控制器。TEB算法在其基础上进行了改进,该方法通过最小化轨迹执行时间、避开障碍物和局部优化机器人轨迹如满足速度和加速度的限制使其符合运动学约束,可以进行在线避障和路径规划。很多研究者对其进行了运动控制方面的改进,如文献[12]将TEB算法与混合速度倒数障碍模型相结合,使其考虑了机器人和周围移动障碍物之间的潜在碰撞,实现主动避障。文献[13-14]在TEB算法中引入了社会学约束,将人类的社会时空特征(包括人类的位置、与机器人的相对运动以及社会规则)整合到传统的TEB在线轨迹规划算法中,使其可以在动态的社会环境中安全地避开人类。郑凯林等[15]在TEB算法中引入了加速度约束,通过使加速度在恰当的范围内变化,避免了阿克曼机器人在运动中产生的轨迹突变和速度震荡。文郁等[16]在TEB算法中加入了危险惩罚因子约束、加速度跳变抑制约束和末端平滑约束来使机器人在障碍物密集处的运动轨迹更加安全。而陈奕梅等[17]对于多机器人在动态障碍物环境中规划路径不佳且速度不稳问题,融合了TEB算法和速度障碍(VO)法,提出了TEB-VO算法,同时设计了自适应参数动态调节模块从而使得机器人在靠近动态障碍物时输出速度更加平缓。

由于在使用当前的TEB算法进行路径规划的实验时,总会发现遇到静态障碍物或者附近存在动态障碍物时机器人会出现骤停和突然加速的速度震荡情况,缺乏鲁棒性。为了克服这个问题,本文在TEB算法基础上提出了基于曲率速度控制的时间弹性带算法(Curve Speed Control-Time Elastic Band, CSC-TEB),该算法的中心思想是能够使机器人根据轨迹弯曲程度来控制其线速度,在遇到障碍物时适当降低速度大小,从而减少速度震荡的频率,提升避障平稳性,具有计算简单、执行效率较高的优点。

1 TEB算法

1.1 TEB算法的基本原理

早期的EB算法使用世界坐标系,定义了一系列的机器人位姿Si={xi,yi,θi},其中xi和yi表示机器人的方向,θi表示机器人的方向角,因此EB算法的机器人轨迹可以表示为n个位姿序列的集合,如式(1)所示:

Q={Si}i=0,1,…,n。

(1)

TEB算法在EB算法基础上加入了时间T,由位姿Si运动到Si+1之间的时间间隔为ΔTi,n个位姿序列生成了n-1个时间间隔,由此可得到整个运动过程所有时间间隔的集合如式(2)所示:

τ={ΔTi}i=0,1,…,n-1。

(2)

机器人在运动过程中,都是按照顺序由前一个位姿运动到随后的一个位姿,故机器人在局部路径规划算法内的轨迹是由位姿序列和时间间隔序列2个集合共同组成的,如式(3)所示:

B:=(Q,τ)。

(3)

机器人运动轨迹如图1所示。

图1 TEB 路径位姿和时间间隔关系Fig.1 The relationship between the mobile robot’s postureand the time interval in the TEB algorithm

整体算法是通过多目标优化来获取最优路径点集合,最优轨迹B*通过最小化一个加权非线性最小二乘函数来获得,如式(4)和式(5)所示:

(4)

(5)

式中:f(B)为考虑各种约束的目标函数,γk为目标函数的权值,B*为最优轨迹结果。

TEB算法已经存在的约束有4种,跟随路径以及避障、速度和加速度约束、非完整运动学约束以及最快路径约束,具体如下:

① 跟随已经确定的全局路径,所跟随的路径施加外力将整条“橡皮筋”拉向全局路径,而避障可以理解为障碍物施加外力使其远离障碍物;

② 因为其速度/加速度只与移动机器人的连续状态有关,所以可以通过差分计算来进一步得到,并且根据原有的性能约束,速度/加速度具有最大最小值限制;

③ 对于移动机器人的走向轨迹应该是若干段弧组成的平滑轨迹,不应有漂移到轨迹之外的点;

④ 要尽可能减少所要花费的时间,从而用最短时间目标代替其他算法中的最短路径目标。

1.2 TEB超图的构建

TEB算法具有局部规划性,每一次的迭代只需要插入一个新的位姿状态或删除一个旧的位姿状态,从而保证了整个优化过程的求解速度,并且也保证了机器人的实时避障能力。TEB算法中规划的路径点与障碍物之间及相关条件的约束关系可以用超图[18]表示,超图的边表示目标函数和约束代价,顶点包括移动机器人的位姿、相邻位姿的时间间隔以及障碍物。

传统TEB算法的超图如图2所示(其中展示出了机器人在4种相邻位姿间转换时的约束关系)。

图2 TEB超图Fig.2 TEB hyper-graph

超图作为一个开源框架,对于具有特殊结构的非线性优化问题是可以解决的。这个框架将轨迹优化问题构建成了一个g2o图优化问题,需要定义节点和边,对于每个节点会定义相应的增量,并通过g2o中关于大规模稀疏矩阵的优化算法解决。即如图2所示,将机器人的位姿描述为节点(vertex),而目标函数以及设定的约束函数作为边(edge)。

2 CSC-TEB算法

2.1 CSC-TEB算法设计

引入速度控制因子αv,如式(6)所示:

αv=1-Δθ/π。

(6)

移动机器人从第i个姿态Si运动到第i+1个姿态Si+1的轨迹示意如图3所示。di,i+1为Si和Si+1这2个姿态之间的直线连线,也称为机器人运动的目标方向,θi与θi+1分别为机器人在姿态Si和Si+1时速度方向与下一目标方向的夹角,也称为速度偏角,Δθ为移动机器人的第i+1个姿态Si+1的速度偏角θi+1与第i个姿态Si的速度偏角θi之差。

图3 速度偏移角夹角示意Fig.3 Diagram of velocity offset angle

根据实际情况,移动机器人的最大速度偏角只能达到π,即在朝下一目标点完全相反方向运动时取得,因此式(6)中的Δθ∈(0,π),即αv的取值(0,1)。

TEB算法中的速度约束只由速度的上限值和下限值来决定;而CSC-TEB算法进行了改进,即机器人避障转弯时的最大瞬时速度可由速度控制因子αv进行调节,如式(7)所示:

vi=vimax×αv(i=1,2,…,n),

(7)

由式(7)可以看出,机器人的最大瞬时速度vi在原来最大速度vimax的基础上由αv加权,而αv与Δθ成反比关系,当移动机器人行进的轨迹曲率变大时,即它的行进方向偏离目标点直线距离的程度变大时,速度控制因子就可以发挥降低移动机器人速度的作用。

2.2 CSC-TEB算法实现

为了实现上述速度控制因子αv的功能,添加了轨迹曲率与速度之间的约束,在TEB超图中加入速度控制因子αv,改进后的TEB超图如图4所示。

[46] East Asia Forum, Chinese security and freedom of navigation in the South China Sea, June 5, 2017, http://www.eastasiaforum.org/2017/06/05/chinese-security-and-freedom-of-navigation-in-the-south-china-sea/.

图4 改进后的TEB超图Fig.4 Improved TEB hyper-graph

在上图中,速度控制因子αv作为轨迹曲率与移动机器人的速度之间的约束,该约束在超图中转换成代价函数,从而在局部路径规划过程中起到在避障时实时调整机器人运动速度的作用。

CSC-TEB算法参与的整个路径规划算法实现遵循如下步骤:

① 在全局规划和局部规划配置文件中设置好相应参数,制定目标路径的起点和终点,由move_base调用navfn包中的全局路径规划算法进行全局路径规划;

② 全局路径规划完成后,反馈给move_base,move_base进一步调用base_local_planner包中的局部路径规划TEB算法进入局部路径规划;

③ 选取全局路径规划上的部分路径点作为本次避障过程局部路径规划的路径点,记录移动机器人的初始化轨迹以及位姿,得到每次迭代的时间间隔;

④ 将规划路径点所形成的航向角与移动机器人的速度之间用速度控制因子αv进行关联,根据相邻路径点计算机器人的行进速度和速度偏角等控制变量,最大速度由速度控制因子进行约束,根据上述步骤构建完整超图,找出最优轨迹;

⑤ 更新环境地图中机器人与障碍物位置信息;

⑥ 判断是否到达终点;

⑦ 到达终点算法结束;没有到达终点则返回③。

3 仿真实验

为了分析并验证CSC-TEB算法的优越性,将CSC-TEB算法与TEB算法分别在静态与动态场景中进行了仿真。计算机配置为:CPU i7-7700 主频 2.80 GHz,4核8线程,操作系统为Ubuntu 18.04,采用ROS机器人仿真平台[19],2轮差速机器人进行算法的仿真。仿真的初始参数配置如表1所示。

表1 机器人运动学参数

3.1 静态环境下的仿真实验

在同一静态场景下对TEB算法和CSC-TEB算法用于2轮差速机器人的导航。Rviz可视化平台中的仿真场景如图5所示,图中存在2个圆形静态障碍物。在该场景下应用TEB算法和CSC-TEB算法都选择了相同的最优路径,即中间带红色箭头的路径,但速度曲线呈现出区别如图6所示。

图5 仿真环境Fig.5 Simulation environment

(a)TEB算法速度曲线

(b)CSC-TEB算法速度曲线

图6(a)和图6(b)分别展示了通过圆形障碍物时TEB算法和CSC-TEB算法的线速度曲线,通过这2幅图在相同场景下的对比,TEB算法一直以最大速度行进,而CSC-TEB算法在轨迹曲率较大的情况下,可以使移动机器人根据曲率来适当降低速度,从而达到安全平稳地度过弯道的目的。

为了使可视化效果更加明显,本文亦在Stage平台下对TEB算法和CSC-TEB算法进行了对比仿真实验。Stage平台中TEB算法和CSC-TEB算法规划的路径如图7所示,其中图7(a)和图7(b)中的红色部分分别为2种算法在转弯时所规划轨迹的效果,可以发现TEB算法所规划的运行轨迹在转弯时会贴着转角前进,不能完全保证机器人的运行安全;而CSC-TEB算法所规划的运行轨迹是在一个安全区域内行进的,轨迹更加平滑。

(b)CSC-TEB算法

综上可以得出,CSC-TEB算法可以保证机器人在有效避障的前提下,对最大瞬时速度进行适时调整,轨迹更加安全平滑。

3.2 动态环境下的仿真实验

在完成静态环境下的仿真实验后,为了更好地展现出在动态环境下CSC-TEB算法的平稳性,在仿真环境下加入了做匀速运动的具有规则形状的动态障碍物,该障碍物做着反复的周期运动。而面对具有不规则形状的障碍物,可以使用已经设置好的膨胀半径对其做膨胀化处理,有效保证移动机器人的安全性。

结合该局部路径规划算法所针对的实时躲避障碍物的特性以及本文的CSC-TEB算法进行综合考虑,需要对具有不同表面形貌的动态障碍物进行实验。本文设置了多个不同大小、且形状不规则的障碍物进行实验,其中约50%的障碍物使得机器人产生较小的转弯曲率,约50%的障碍物使得机器人产生较大的转弯曲率,具体实验环境如图8所示。

(a)移动机器人具有较大转弯曲率

(b)移动机器人具有较小转弯曲率

在保证起点、终点和移动障碍物的运动状态一致的前提下,用TEB算法和CSC-TEB算法在该动态环境下进行多次仿真实验。对移动机器人从起点到终点的全部转向角取绝对值后进行求和运算。实验结果如表2所示。

表2 转向角总和

实验数据表明,当移动机器人躲避移动障碍物而转弯较小时,TEB算法和CSC-TEB算法体现出的性能相差不大。但是,当转弯较大时,在TEB算法下的转向角之和远大于CSC-TEB算法的转向角之和。

上述实验结果表明,移动机器人转弯幅度较大时,在TEB算法下迫使移动机器人产生更多曲折的路径才能安全地躲避障碍物,移动机器人会因为需要重新规划速度而花费更多的时间,降低完成整体路程的效率。而在CSC-TEB算法中,移动机器人轨迹更加平滑,使用CSC-TEB算法的移动机器人更加适合社交导航。

在图8(b)的实验场景下,在保证起点、终点和移动障碍物的运动状态一致的前提下,截取了整体路径规划的速度变化曲线,如图9所示。可以看到当采用CSC-TEB算法时机器人对于动态障碍物的躲避是相对平稳的,速度的跳变次数较少,整个避障过程更加稳定,而TEB算法产生了多次的速度震荡。

(a)TEB算法速度震荡情况

(b)CSC-TEB算法速度震荡情况

在该次实验中也对二者的速度震荡次数、避障时延和路径耗时进行了统计,数据如表3所示。

表3 速度震荡次数和路径耗时

表中数据是经过100次实验后得出的数据。使用TEB算法的移动机器人在转弯时仍然是以最高线速度行驶,随着实验次数的增多,可以看到它在遇到动态障碍物时,速度震荡次数较多,并且突变范围较大。而在CSC-TEB算法下转弯时线速度变小,运动情况相对平稳,避障时延相对较短,并且路径总耗时也较TEB算法降低了15.02%,基本达到预期要求,证明了本实验的合理性。

通过上述静态与动态场景下对TEB算法的仿真可知,CSC-TEB算法能够使机器人在遇到障碍物时根据转弯程度降低速度,减少了路径重新规划次数,减少了速度震荡和轨迹抖动现象,能够实现平稳避障,节省了避障时延和整体路径耗时。

4 结束语

本文对移动机器人在实时环境中的局部路径规划算法——TEB算法进行了研究,并在其基础上提出了CSC-TEB算法。所提出的算法对机器人在避障过程中遇到具有弯曲程度较大障碍物时的速度进行了考虑,使其速度受轨迹弯曲程度的约束,最大瞬时速度可以适时调整。经过静态环境和动态环境仿真实验的对比,证明了CSC-TEB算法能够在成功避开障碍物的前提下,保证移动机器人行进过程中的安全性,提升了机器人的轨迹平稳性,同时在一定程度上节约了完成整个路径规划的总体时间延迟。