改进TEB算法的局部路径规划算法研究

代婉玉，张丽娟，吴佳峰，马向华

上海应用技术大学 电气与电子工程学院，上海 201418

移动机器人是否具有自主感知[1]、能够进行自主导航[2]是当前人工智能领域研究的热点。但现如今移动机器人的路径规划不仅要求利用算法找到一条从出发点到目标点的无碰撞路径，还要求机器人能够在复杂动态环境中完成自适应避障。只使用全局路径规划算法显然不能适用于复杂环境下的机器人，需要局部路径规划算法与之配合。但是当前几种常用的局部路径规划算法还存在一些缺陷：人工势场法[3]会使移动机器人陷入局部极小值和不可到达点。动态窗口法[4]只适用于全向移动机器人和差速移动机器人，并不适用于所有类型的机器人。随机搜索树算法[5]随机性较大，导致最终生成的搜索路径具有概率性。EB（elastic band）算法[6]应用在动态环境中，该算法根据障碍物的位置和自身位姿信息使局部变形，使机器人同时受到相互制衡的拉力和排斥力，重新规划一条避开障碍物的路径。后EB算法中加入了对机器人的非完整运动学约束[7]，但始终没有引入时间信息。

基于以上的研究，近几年Röosmann等[8]新提出的TEB（time elastic band）算法是在EB算法的基础上，加入了时间信息的思想，还考虑到了机器人的动力学和非完整约束，使其在两轮机器人[9]和四足机器人[10]的适应性都很强。将路径规划问题转为图优化的问题，是动态系统中一种有效的时间最优模型预测方法[11]，计算效率高、对全局规划的要求小，广泛应用于物流领域和自动驾驶领域[12-13]。但在实际应用过程中还是会存在速度跳变和机器人易受冲击等问题。国内外许多学者对传统TEB算法做出了一些改进措施。

文献[14]在求解过程中对问题施加连续优化约束。文献[15]加速度的变化率过大引起机器人受到冲击震荡，加入了加加速度的概念。文献[16]在TEB算法中新加入了加速度变化率约束以改变障碍物比重。文献[17]相比TEB算法最大速度与最大角速度的约束，在原有算法的基础上进一步增加了速度与角速度的联合约束。上述文章只是加入了机器人的速度约束，没有考虑到障碍物速度过快时易发生碰撞。文献[18]为了改进最优轨迹的选取方法，在基于拓扑结构的并行TEB轨迹优化方法的基础上，增加了候选轨迹评分函数。文献[19]通过增加危险惩罚因子约束，增加加速度跳变抑制约束、增加末端平滑约束。上述文章增加并行轨迹和增加过多约束会导致算法的计算量激增，对机器人的处理器要求极高，并不适合批量生产，在实际复杂环境中应用性不高。

根据上述分析中算法存在的问题，对TEB算法的进行更适合于复杂环境下的改进。在原有的TEB算法中加入遇到障碍物后的膨胀化处理，使其适用于不同形状的障碍物环境中。针对碰撞安全性，对机器人可运行区域划分等级。加入速度与障碍物距离的约束来降低速度跳变导致的规划路径不平滑。为验证算法可行性，分别在仿真中设置静态环境和动态环境用传统TEB算法与改进TEB算法进行对比。

1 传统TEB算法的路径研究规划

1.1 算法基本描述

TEB算法用机器人的n个位姿序列排列而成，x i、yi表示机器人位置，βi表示机器人的方向。机器人的位姿有序序列为：

TEB算法在EB算法的基础上加入了时间间隔ΔT，是机器人从当前位姿运动到下一个位姿需要的时间，通过合并连续位姿的时间间隔，生成一个时间间隔序列：

机器人的轨迹可表示为：

机器人速度和加速度约束用几何约束的惩罚函数实现的，移动线速度和角速度是根据两个时间间隔为ΔT的连续位姿x i和x i+1之间的距离和的角度变化来计算的。

加速度的约束是根据两个连续的平均速度计算，平移速度通过欧氏距离单位时间内的变化量计算，需要考虑3个连续位姿，和它们相应的两个时间间隔：

TEB算法相较于EB算法考虑到了时间的约束，尽量减少花费的时间成本，该算法规划出的路径以时间最优为目标代替其他路径规划算法以路径最短为目标，可使机器人花费最短的时间到达目标点。时间约束的目标函数如式（6）所示：

1.2 TEB超图的构建

TEB算法中产生的稀疏系统矩阵问题用位姿和时间间隔为节点的Hyper-Graph（超图）表示。将目标函数和约束代价函数用边来描述，每个节点由对应的边连接成为超图。如图1所示为轨迹优化场景下需要的目标函数TEB超图，超图中的约束主要包括障碍物、航路点、机器人自身的速度、加速度和非完整约束等。每条边对应的约束函数根据当前的环境进行约束，并将权值加入整体约束函数中来进行路径规划。

图1 TEB超图Fig.1 TEB hypergraph

算法以高于机器人相应周期的频率不断更新。在每一次更新中，算法都会添加着新的位姿并删去旧的位姿。对于每一个周期检测到的障碍物来说，算法将其视为静态障碍物来处理。每一次的周期迭代都会产生一个新的超图。

在超图中，速度的约束连接着机器人连续的两个位姿Si和Si+1和一个时间间隔Ti；加速度的约束连接着机器人连续的三个位姿Si-1、Si和Si+1和两个连续的时间间隔Ti和Ti+1；障碍物约束Fob连接着障碍物和距离障碍物最近的机器人位姿节点Si；航路点约束Fpath连接着机器人当前位姿节点Si和距离相邻航路点。

2 改进TEB的约束条件

传统的TEB路径规划算法是通过实时环境探测，探测到的环境信息不断进行反馈，根据反馈结果实时优化路线，实现了在未知和已知环境下的局部避障功能。但是在实际应用过程中还是会存在部分问题。比如在复杂动态环境中，在狭窄通道内遇到数个动态障碍物陷阱导致道路不可通行时，TEB算法可能存在局部最小或不可到达的情况。如果在机器人避障过程中，障碍物在与机器人距离比较接近时，运动速度突然过快，两者有可能发生碰撞。传统TEB算法没有考虑到障碍物的形状，在遇到不规则障碍物后，避障过程需要大量的运算，还可能对机器人的安全性造成威胁。避障过程中，过快的速度变化导致控制量不平滑，路径行驶中的不流畅。

2.1 对障碍物做膨胀化处理

针对在传统TEB算法中障碍物惩罚函数因不规则障碍物而不断变化，使计算量激增，对TEB算法进行改进。建立不规则障碍物的膨胀模型，用已经设定好的膨胀半径对障碍物做膨胀化处理，来减少算法的计算量，使移动机器人获得更加平稳安全的轨迹，对障碍物的膨胀化处理如图2所示。

图2 将障碍物做膨胀化处理Fig.2 Expanding obstacles

2.2 区域分级策略

针对在传统TEB算法中障碍物和机器人距离过近时，因障碍物速度变化可能会发生碰撞，无法保证机器人安全性的问题，提出使用一种区域分级情况将以机器人与障碍物距离为半径，在路径规划中遇到障碍物的距离分成安全区域、相对碰撞区域和绝对碰撞区域。3种区域分别如图3（a）所示。

如图3（b）所示，传统TEB算法规划路径时，与障碍物距离过近，考虑到机器人在实际复杂环境中的不确定性，提出对TEB的改进算法。改进后的TEB算法进行动态避障时，优先考虑可在安全区域规划的路径。如果在安全区域没有可行路径，则考虑相对碰撞区域的可行路径。如果还是没有可行路径，路径规划失败。

图3 障碍物所在区域Fig.3 Obstacle area

实现分级的意义有以下几个方面：（1）增加机器人避开障碍物的可能性，保证机器人运动过程中的安全性；（2）在机器人检测到障碍物的过程中，优先选择安全区域的路径，可以大量减少TEB算法的计算量，提高实时性；（3）在狭窄通道中，如遇动态障碍物，机器人大部分情况下都会处于相对碰撞区域和绝对碰撞区域，通过改进的算法速度权重，更加智能地避开障碍物，增加机器人通过动态障碍物场景的可行性。

2.3 增加速度的约束

如果障碍物出现在相对碰撞区域内，就会认为将来一段时间内机器人会与障碍物发生碰撞，虽通过速度迅速下降直至停下，等障碍物离开相对碰撞距离后再继续行进的策略会实现避障，但是这样会导致机器人在遇到动态障碍物后的速度跳变，速度控制量不平滑，增加不必要的消耗。考虑到机器人的成本和安全性，本文加入速度与障碍物距离约束关系，现将障碍物与速度在超图中连接，构建新的超图，如图4所示。

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

线速度在加入障碍物的约束后可表示为：

同时，角速度也可表示为：

2.4 改进TEB算法实现过程如下：

改进TEB算法的路径规划过程如下所示：

（1）初始化运行参数，输入全局路径，给出起始点和终点；

（2）调用一段全局路径，得到初始化轨迹，机器人的位姿状态，每次迭代的时间间隔；

（3）根据这一段初始化径规划路线，插入/删除TEB状态；关联TEB状态到路径点和障碍物点；

（4）生成超图并优化超图，找出最优轨迹；

（5）判断当前轨迹是否在安全区域；

（6）如在安全区域就继续行进，不在安全区域就判断安全区域的可行性；

（7）安全区域没有可行性就在相对碰撞区域找最短轨迹路线，同时加入机器人速度与障碍物的约束；

（8）根据相邻的导航点计算机器人的行进速度和转向角等控制变量；

（9）更新环境地图中机器人与障碍物位置信息；

（10）是否到达终点；

（11）到达目标点则结束算法，没有到达则返回步骤（2）。

3 仿真实验与分析

为验证上述改进TEB算法可行性与有效性，在ROS系统中设置了仿真程序，分别对TEB传统算法和改进算法仿真验证。仿真平台为Ubuntu系统中的Rviz，ROS导航功能包通过配置文件完成相应的参数配置，相关参数如表1所示。仿真所用的机器人参数通过多次调试获得。

表1 机器人运动学参数Table 1 Robot kinematics parameters

改进后的TEB算法的局部路径规划实验主要分成两种情况进行仿真：（1）静态环境；（2）动态环境，其中包括简单动态环境和复杂动态环境。主要目的是验证改进TEB算法在静态和动态环境中是否能更安全有效地规划路径，并且能够减少机器人受到的冲击。

3.1 静态环境中的轨迹安全性分析

如图5所示，（a）、（c）代表在静态环境中传统TEB算法的运行轨迹，虽路径短，但是会贴着障碍物运动，无法保证机器人的安全性能。图5（b）、（d）代表在静态环境中改进的TEB算法的运行轨迹。可以看出，改进的TEB算法为了保证器人的安全性，使机器人与障碍物之间保持安全距离，优先考虑在安全区域的路线规划。

图5 静态环境中的局部路径规划Fig.5 Local path planning in a static environment

3.2 动态环境中的路径规划

在静态环境中的仿真实验完成后，为了更好地表现出机器人在车间环境下的可行性。在ROS仿真环境中，分别构建一个单障碍物动态环境和一个多障碍物有动态环境，该环境中行驶的机器人设定为双轮差速机器人，加入提前构建的动态障碍物，进行周期性的上下移动，以模拟机器人的动态工作环境。为了能更加清晰直观地表达实验效果，同时在stage平台上进行同步模拟仿真。

在构建的仿真环境中，给机器人一个导航的目标点，用传统的TEB算法和改进后的TEB算法分别在简单动态环境和复杂动态环境进行仿真实验。如图6（a）和图7（a）所示，传统的TEB算法规划的路径会贴着障碍物运动，机器人与障碍物碰撞的可能性较大。而改进的TEB算法中，优先选取在安全区域状态下的局部路径，增大与障碍物之间的距离，保证机器人运行的安全性。在无安全路径时选择最短路径，同时增加障碍物与机器人速度的约束，规划出新的局部路径。通过图6（b）和图7（b）所示，机器人与障碍物保持着相对安全的距离，提高了机器人运行过程中的安全性。

图6 简单动态环境中的局部路径规划Fig.6 Local path planning in simple dynamic environment

图7 复杂动态环境中的局部路径规划Fig.7 Local path planning in complex dynamic environment

3.3 动态环境中的速度变化曲线

如图8所示，蓝线为图6中动态障碍物循环往复运动速度的变化图。红线为图6中机器人运行中的速度变化图。图8（a）为在传统TEB算法中形成的速度曲线，图8（b）为在改进后的TEB算法中形成的速度曲线。在传统TEB算法和改进后的TEB算法仿真中设定相同的起止点后，两种算法生成的速度变化图有明显区别，改进后的算法在应用过程中速度变化明显变慢。具体数值如表2所示。

图8 机器人与障碍物的速度变化图Fig.8 Velocity variation diagram of robot and obstacle

表2 速度曲线分析Table 2 Velocity curve analysis

仿真测试结果表明：在传统TEB算法的基础上，通过对速度进行一个新的约束，即增加了速度与障碍物距离的约束，形成了改进的TEB算法。通过在静态环境中和动态环境中的仿真可以看出来，使用改进的TEB算法后，速度震荡次数在X轴和Y轴分别减少40%和81%，速度差值也分别减少了62.5%和22.2%。速度变化曲线明显平滑，速度变化减慢，基本达到预期要求，证明本实验设计的合理性。

4 结论与展望

本文针对障碍物在动态环境中机器人的局部路径规划问题，对TEB算法局部路径规划算法进行了优化，提出了一种改进的TEB算法。所提出的算法改进考虑到了障碍物不规则的形状及速度过快导致碰撞的可能性，对障碍物膨胀化处理并对机器人可行区域分级处理。然后用障碍物距离对机器人速度进行约束。通过在静态和动态环境中的仿真对比实验发现，改进的TEB算法增加了机器人行进过程中的安全性考虑，减少速度跳变导致的速度控制量不平滑，提高机器人动态环境中运行的平滑性，得到的轨迹可有效避开障碍物。