融合改进A∗和时间弹性带的移动机器人路径规划算法

沈斯杰，田昕，袁千贺

（1 上海理工大学 光电信息与计算机工程学院，上海 200093；2 上海理工大学 理学院，上海 200093）

0 引言

随着智能制造2025 和工业4.0 的提出，机器人在化工、纺织、食品、电厂、农业等领域应用得越来越多。路径规划是机器人控制系统的基础问题，是机器人顺利完成各项作业任务的前提条件［1］。

按照对周围环境信息的熟悉程度的不同，路径规划主要可分为2 类：

（1）全局路径规划。是指在已知机器人的全局环境信息的条件下，找到一条起点和终点之间距离最短的路径，且该路径不经过任何障碍物［2］。常见的全局路径规划算法有：D∗算法［3-4］、A∗算法［5-6］、Dijkstra 算法［7-8］等。

（2）局部路径规划。描述了如何让机器人遵循全局路径，同时避免未知的障碍物［9］。常见的局部路径规划算法有：DWA 算法［10-11］、TEB 算法［12-13］、人工势场法［14-15］等。

在实际应用中，单一算法在解决路径规划问题时，具有局限性，并且无法处理机器人在移动过程中面对的突发情况。因此，将不同算法进行融合成为当下路径规划中的研究热点，通过算法之间的优劣互补，从而得到较优的规划结果［16］。王志中［17］通过融合改进A∗算法和人工势场法，在复杂环境下实现实时避障，但规划的路径在障碍物处不够平滑，会增加机器人转弯和变速的负担。Dai 等人［18］通过将改良的蚁群算法和A∗算法融合，提升算法的收敛速度和全局路径的平滑度，但是没能顾及机器人在移动过程中出现的未知障碍物。庞磊等人［19］提出将A∗与TEB 算法融合，帮助机器人实现对目标行人的安全无碰撞跟随，然而A∗算法规划出的路径不够平滑，转折点多，不适合机器人长距离移动。

针对上述情况，为了提高A∗算法的路径平滑度，同时满足机器人在复杂环境下实时避障的需求，提出一种融合改进A∗和TEB 算法的路径规划方法。首先，通过A∗算法进行全局路径规划，得到适合机器人平稳运行的全局路径。其次，为了使TEB算法更遵循全局路径，增加了路径点与TEB 轨迹位姿点的约束条件。最后，如果机器人在移动时通过自身携带的传感器感知到全局路径上有障碍物出现，则TEB 算法能够实时调整局部路径，从而绕开障碍物。完成避障后，继续沿着全局路径前行至目标点。

本文的贡献可以归纳为：

（1）引入梯度下降算法的思想，减少路径转折点，从而提高路径平滑度。

（2）添加了路径点与TEB 轨迹位姿点的约束条件，使TEB 算法规划的局部路径更好地跟随全局路径。

（3）在复杂环境下，通过将改进A∗算法与TEB算法相融合，有效躲避未知的障碍物。

1 改进A∗算法

1.1 A∗算法

A∗算法作为一种在全局环境信息已知的情况下寻找最优路径的启发式算法，从起始点开始，将起始点作为父节点，通过向周边扩展，计算得到周围8个子节点的评价函数值，选取最小值子节点作为下一轮扩展的父节点，以此往复，直至到达目标点，从而得到最优路径。其评价函数为：

其中，f（n）是机器人从起始点经过节点n到目标点的总代价值；g（n）是机器人从起始点到节点n的实际代价值；h（n）是机器人从节点n到目标点的估计代价值，又被称作启发函数；n是机器人当前所在的节点。

1.2 平滑路径

A∗算法虽然能够在静态环境中得到最优的全局路径，但规划的路径存在曲折点较多的现象，会增加机器人转弯、变速的负担。针对这一情况，利用梯度下降法对搜索出的路径进行平滑处理，从而使A∗算法规划的路径更适合机器人在实际的场景下运行。

梯度下降法常应用于求解最小二乘法问题，本文中将对搜索出的路径做平滑处理，其优点为计算量较少，能够最小化所有样本损失函数值，并且使结果为全局最优解或者在最优解附近［20］。梯度下降法作为迭代法的一种，利用逐步迭代的方法得到评价函数的最小值，从而实现拟合曲线的功能。

首先，将A∗算法规划出的路径近似为曲线，并采用函数的形式表示：

其中，xn为数据样本中第n个特征值，θn为第n个特征值的权重。

接着，引入一个假设函数y′，将一组特征值代入上式中，因为各个权重值尚未确定，故y′不等于y。假设函数y′的表达式为：

然后，将预测值y′与真实值y做差，为了保证差值是正数，再对差值取平方，即可得到损失函数ΔT（θ），其表达式为：

损失函数的值越小意味着，预测值和真实值之间的误差越小，因此损失函数越小越好。针对原始路径的数据样本存在样本数量较大的情况，采用均方差的形式来表示损失值，其表达式为：

其中，J（θ）为均方差损失，涵盖了所有样本数据的表现。为了便于后续计算，在等式的右边乘以二分之一，即得：

梯度下降法采用微分的思路进行求解，将权重θ的取值范围分割成无限份，每一份的宽度为动态∂，其实际宽度由微分步长α决定，通过不断改变权重值，使得损失值J（θ）趋近于0。

由微分公式可以变形成权重θ的迭代公式，其表达式为：

关于原始路径的样本数据，仅需要改变权重值的大小和迭代的频次，就能获得目标函数的最小值，从而达成平滑处理路径的目的，如图1 所示。

图1 路径平滑处理Fig.1 Path smoothing

图1中，蓝色的点划线为原始路径，红色的虚线为通过梯度下降法平滑处理后的路径。从图1 中可以明显看出，原始路径转折点较多、不光滑，而利用梯度下降的方法能有效减少转折点，路径平滑度得到大幅提高。

2 改进TEB 算法

2.1 TEB 算法

全局路径规划是针对静态环境进行点到点之间的路径规划。如果路径中突然出现障碍物，则无法躲避。而局部路径规划可以通过传感器实时采集周围环境信息，从而知道机器人在静态环境中所处的位置和突然出现的障碍物的分布情况。

TEB 算法利用图优化的思想，将影响弹性带形变的机器人位姿点与经过相邻两位姿点所需的运动时间定义为需要优化的节点，并且将路径起始点、机器人的速度、加速度和周围环境信息的约束定义为需要优化的边，采用g2o 开源框架进行求解。TEB算法的设计步骤详见如下。

首先，将机器人的位姿定义为：

其中，xi和yi是机器人所处空间的位置坐标，βi为机器人运动过程中的朝向信息。机器人位姿组成的序列定义为：

其次，将机器人经过相邻位姿所花费的时间定义为ΔTi，由一个个时间间隔组成的序列定义为：

然后，将TEB 算法所需要优化的目标表示为：

最后，将加权求和得到的全局约束函数f（B）对多目标进行优化，表示形式为：

其中，fk（B）为局部约束函数；γk为约束函数所对应的权值；B∗为TEB 算法优化后满足约束函数的最优解。

机器人的位姿与时间间隔的关系，如图2 所示。

图2 机器人的位姿和时间间隔的关系Fig.2 Relationship between pose and time interval of robots

2.2 构建TEB 超图

超图作为一种特殊的图，可以更加准确地描述多因素关联的对象之间的关系。TEB 算法的局部性会导致稀疏性系数矩阵问题的产生。这一问题可以转换成由位姿和时间间隔作为节点、目标函数和约束函数作为边构成的超图［21］，如图3 所示。这是在路径规划情况下，由TEB 算法的约束转变而来的超图。超图的约束主要包含机器人自身的几何约束、速度、加速度、时间最优和相邻障碍物，图3 中起始位姿、终点位姿以及障碍物的节点是不能变动的，用双圈表示。

图3 TEB 超图Fig.3 TEB hyper graph

2.3 增加路径点与TEB 轨迹位姿点的约束

移动机器人在转弯时，TEB 算法规划出的局部路径易偏离全局路径，与墙面或者障碍物距离较近。这无疑增加了机器人过弯时的危险性。为了缓解这种情况，在超图中加入路径点与TEB 轨迹位姿点的约束关系。路径点的集合是在规划范围内全局路径点集合根据相同步长求得的子集合。当TEB 算法在进行优化时，将会受到路径点的约束，需要求解出与各个路径点距离最短的TEB 轨迹位姿点。因此TEB 算法规划出的路径更好地遵循全局路径。路径点与位姿点之间的欧式距离，记作drpmin。那么路径点的目标函数的表达式为：

此时，将路径点与TEB 轨迹位姿点加入超图中，新的超图如图4 所示。

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

3 融合算法

A∗算法可以在静态地图下得到全局最优路径，但是不能实时躲避未知障碍物。TEB 算法作为局部路径规划算法，由于缺少全局环境信息，规划的路径可能不是最优路径，并且存在得到错误路径的风险。所以，通过融合改进A∗与TEB 算法，不仅能够得到全局最优路径，而且可以实时避障。融合算法的流程如图5 所示。

由图5 可知，首先，通过改进A∗算法对静态地图进行全局路径规划，得到适合机器人运行的平滑路径。其次，将规划的路径离散化为时间序列和位姿序列，并将其作为TEB 算法的轨迹序列点。然后，通过g2o 库求解得到最优路径，作为局部路径。如果该路径无效，则重新使用A∗算法进行全局路径规划。如果有效，则将速度指令发送给运动控制单元。最后，机器人沿着全局路径，朝着目标点移动，如果当前位置为全局目标位姿，则路径规划任务完成。反之，则继续进行局部路径规划。

图5 融合算法流程图Fig.5 Flow chart of fusion algorithm

4 仿真实验验证

4.1 改进A∗算法仿真实验

为了检验改进A∗算法的有效性和可行性，本文在Ubuntu 系统下对相关算法进行仿真实验。实验平台为戴尔工作站，配备Ubuntu16.04 操作系统和ROS（kinetic）机器人操作系统。仿真实验所使用的计算机配置为：处理器是英特尔Xeon E5-1630 v4，主频是3.7 GHz。

A∗算法和改进A∗算法的路径规划对比图，如图6 所示。为了保证实验的可靠性，进行了多组仿真实验，将多组数据取均值作为最终的结果，性能对比结果见表1。图6 和表1 中的结果表明，未经过梯度下降算法平滑处理的路径，在拐弯处转折点较多，而通过改进A∗算法规划出的路径，转折点显著减少，路径平滑度得到大幅提升。同时也缩短了路径长度，更适合机器人运行。

图6 A∗算法改进前后对比Fig.6 Comparison of A∗algorithm before and after improvement

表1 A∗算法与改进A∗算法的性能指标对比Tab.1 Performance comparison between traditional A∗algorithm and improved A∗algorithm

4.2 改进TEB 仿真实验

将路径点与TEB 轨迹位姿点约束加入后，TEB的轨迹优化如图7 所示。图7中，绿色线段为全局路径，蓝色线段为已行驶轨迹，红色线段是TEB 算法规划的局部路径，蓝色的小方块是路径点。各路径点的间距为0.25 m，由这些路径点构成的全局路径的子集的长度不超过1.5 m。TEB 算法和改进TEB 算法的移动轨迹对比图，如图8 所示，算法改进前后与各拐弯处的最短距离，见表2。图8 和表2中的结果表明，原始TEB 算法规划的最优轨迹在过弯时，容易偏离全局路径，与墙面距离较近；而改进后的TEB 算法与全局路径能保持较高的吻合度，在转弯时，也能较好地跟随全局路径，仍与墙面保持了足够的距离。

表2 算法改进前后与转角的距离Tab.2 Distance from the corner before and after algorithm improvement

图7 改进后TEB 算法路径规划过程Fig.7 Path planning process of improved TEB algorithm

图8 TEB 算法改进前后对比Fig.8 Comparison of TEB algorithm before and after improvement

4.3 融合算法仿真实验

移动机器人的工作场景复杂多变，因此接下去将在仿真环境下，验证融合算法面对未知障碍物时的避障性能。通过改进A∗算法得到平滑的全局路径后，将其离散化为时间序列和位姿序列，在此基础上，改进TEB 算法进行局部路径规划。若携带的激光雷达传感器感知到周围环境中存在未知障碍物，TEB 算法规划的局部路径可修正全局路径，从而达到实时避障的效果。为了检验融合算法的避障能力，在gazebo 仿真环境下进行移动机器人避障实验，仿真环境如图9 所示。

图9 gazebo 仿真环境Fig.9 gazebo simulation environment

仿真实验的过程为：首先确定机器人起始点与坐标点，然后通过A∗算法规划得到全局路径，接着，在机器人沿着全局路径前行的过程中，在全局路径上加入2 个未知障碍物。机器人通过自身携带的激光雷达传感器感知到障碍物，并通过改进TEB 算法规划局部路径，修正全局路径，绕过障碍物。由于具有不同的路径规划空间，全局路径规划算法以1 Hz的频率运行，局部路径规划算法以5 Hz的频率运行。移动机器人避障过程如图10 所示。

图10 融合算法的避障过程Fig.10 Obstacle avoidance process of fusion algorithm

图10中，蓝线是机器人已行驶的轨迹，黑线是改进A∗算法规划的全局路径，黄线是TEB 算法规划的局部路径，红圈圈出的是未知的障碍物。可以看到，改进A∗算法可以规划出较为平滑的全局路径，使机器人能沿着全局路径移动。当机器人靠近障碍物时，TEB 算法规划的局部路径能够修正全局路径，从而绕过未知的障碍物。最终，机器人顺利到达目标位置。从机器人行驶的轨迹可以看出，融合改进A∗和TEB 的路径规划算法可以规划出最优路径，且能够躲避机器人移动过程中出现的未知障碍物，在躲避障碍物时，与障碍物也能保持足够的安全距离。

5 结束语

本文在A∗算法的基础上，提出了一种融合改进A∗和TEB 的移动机器人路径规划算法，有效改善了A∗算法环境适应性差及路径规划不平滑的问题。在A∗算法中，通过梯度下降算法思想，减少大量转折点，使规划的路径更平滑，更符合机器人实际运行的情况；在TEB 算法中，通过增加路径点与TEB 轨迹位姿点的约束，使其规划的路径能更好地遵循全局路径；与TEB 算法相融合，使其具备了实时避障的能力，面对周围环境中出现的未知障碍物，能够有效避障。仿真实验结果表明，所提出的融合算法在移动机器人路径规划中具有一定的可行性和有效性。

在未来的研究工作中，会考虑减少TEB 算法中优化变量的数量，以此降低TEB 算法在局部路径规划中占用的计算资源，并会将融合算法应用于实际机器人中。