CBS框架下面向复杂地图的低拓展度A*算法

宣志玮，毛剑琳，张凯翔

（1.昆明理工大学信息工程与自动化学院，云南昆明 650500；2.昆明理工大学机电工程学院，云南昆明 650500）

1 引言

随着移动机器人的应用范围越来越广泛，移动机器人的应用环境也变得越来越复杂、多变，因此面向复杂地图的移动机器人路径规划问题成为机器人领域［1］、视频游戏领域［2］、无人机领域［3］、机器人编队系统领域［4］的研究热点.目前针对机器人路径规划的求解算法有基于A*（A Star）的算法［5］，基于约简（Reduction-Based）的算法［6］，也有基于搜索框架的算法［7，8］，基于采样的算法［9，10］，人工势场法［11］，蚁群算法［12，13］等.

基于冲突的搜索算法（Conflict-Based Search，CBS）是求解多机器人路径规划（Multi-Agent Path Finding，MAPF）的一种重要框架［7］，由两层结构组成，低层结构采用A*算法完成单机器人路径规划；高层结构进行多机器人路径的冲突判断，并向低层提供冲突点和冲突时间信息.常用A*类算法作为低层寻路算法求解路径，为了提高算法求解效率，其中有Tie-Breaking 策略［14，15］，通过减少搜索过程中拓展节点的数量来提升运行效率.A*算法处理大规模任务时仍然会拓展大量节点，这在计算上是不可行的［16］.

针对低层算法在面对大地图和复杂障碍物时求解效率低的问题，本文提出带障碍处理的Delaunay 三角剖分优化和分段A*算法相结合的思想，对CBS 框架下低层路径规划问题进行求解，以此完成复杂障碍环境中高效地路径优化.

2 问题描述

2.1 Conflict-Based Search(CBS)框架描述

A*算法在求解MAPF 问题时，状态空间与机器人数量呈指数关系［6］，当机器人数量为1 时，状态空间只与地图大小成线性关系.CBS框架将MAPF问题分解为一个带约束限制的单机器人寻路问题.CBS 框架定义如下所示：

给定地图G=(V，E)，V为图中的顶点集合，E为图中的边集合.约束元组信息(ai，v，t)表示机器人ai禁止在时刻t占据顶点v，v∈V.每个机器人最后生成的路径都满足对其添加的约束.

CBS的核心思想是为冲突机器人增加一组约束，直到找到能满足约束的路径.CBS 算法是两层结构算法.在高层，进行全局冲突检测并添加约束；低层则更新机器人路径以满足新的约束信息.如图1所示.

图1 CBS算法对冲突处理的两层结构

2.2 低层机器人路径规划问题描述

机器人的最优路径规划问题为依据某个或某些优化准则（如工作代价最小、行走路径最短、行走时间最短等），在给定工作空间中找到一个从起始点到目标点的无障碍最优路径.

给定栅格地图G和机器人a，机器人起点为s和终点为g，经过时间为T，时间离散化，机器人每次移动时间为一个单位时间，允许向四邻域范围内的自由栅格进行节点拓展，最后即可生成最终路径P为一组路径序列P=(s0，p1，…，pT-1，gT)，注意，该路径集合P为无固定障碍路径.

在CBS 框架下，低层求解器采用考虑时空信息的标准A*算法进行求解.标准A*算法在地形简单、范围较小的地图中寻路求解路径质量高，且在该框架下，A*算法仅与地图规模是线性关系，但随着机器人数量的增加，算法仍然会拓展数量众多的节点，这对求解效率的提升带来了阻碍.

本文通过计算几何和优化启发式函数选择节点策略，两者相结合提高CBS 框架下低层算法的求解效率，加速MAPF的求解过程.

3 带障碍处理Delaunay 三角剖分优化的低拓展度A*算法

为降低A*算法所需的节点扩展度从而提升求解效率，本文提出带障碍处理Delaunay 三角剖分优化的低拓展度A*算法（Low Extension A*algorithm under Delaunay Triangulation with Obstacle Constraints，LEADTOC），采用Delaunay 三角剖分进行地图和固定障碍物的处理［17］，然后采用最短路径算法和可视化方法获得无固定障碍的连通路径，最后采用分段A*算法规划出优化路径.

3.1 带障碍处理的Delaunay 三角剖分及无固定障碍路径优化

Delaunay 三角剖分是指把散点集合C剖分成不均匀的三角形网格.对于平面内的给定散点有以下2 个特点：空圆性（在Delaunay 三角剖分中任意三角形的外接圆范围内不会有其他点存在），该特性保证了Delaunay 三角形的唯一性；最大化最小角特性（剖分后三角形的最小角最大）.

通过缩放因子的灵活选择可控制生成三角剖分地图边和节点的数量，对特定地图选取恰当的缩放因子可在保证求解质量的前提下，进一步优化求解效率，缩放因子一般取为5～8.以250×250 大小的地图为例，取缩放因子为7，可将大地图近似缩放为35×35 大小规模的地图.

本文地图建模时采用栅格法建模.每个栅格与相邻栅格的距离为1.

对于给定地图，首先进行剖分散点的选择，选出两部分的点：包围障碍物边缘的点和二维平面内的均匀取点.包围障碍物边缘的散点，这一部分的散点距离最近障碍物的距离为1，即该部分选取的每一个散点与其最近的障碍物的距离均为1，如图2所示；第二部分散点根据缩放规模在二维栅格地图均匀取点，点间距为缩放因子所确定的缩放规模，缩放规模为5就是在横纵坐标两个维度每隔5个栅格取一个点（不包括处于障碍物内的点），剖分散点由以上两部分点构成.

图2 剖分散点选择

如图3 所示，根据散点生成的第一次三角形，其中部分三角形的边有穿越障碍物的情况.通过最近点搜索算法，将三角形的重心设置为参考点，障碍物点作为查询点，筛选出包围查询点的三角形.后续将筛选出的三角形的每两个顶点间进行线性插值，插值后的状态点有一个及其以上的点处于障碍物栅格则视为这两个顶点之间的边不可见，将三角形组成的不可见边进行删除，最后生成基于三角剖分的无障碍连通图GD（Graph with Constrained Delaunay Triangulation）.

图3 带障碍物处理的三角剖分

随后，对无障碍连通图GD采用最短路径算法计算从起点s到终点g的最短路径，可获得机器人a在图GD上的路径集合P=(s，v1，…，vn，g)，vi（i=1，…，n）为中间路径点，s，g，vi∈V.

根据两点间直线距离最短原理，采用可视性判断对路径集合P从起点s到终点g进行优化，将当前节点与序号大于当前节点的最大序号可视节点连接，连接后将当前序号最大的可视节点视为起点与其后续节点继续做可视性分析，最终得到优化后的路径集合.如图4 所示，通过最短路径算法得到路径集合P=(s，v1，v2，v3，v4，g)，经过可视性优化后，得到路径集合Popt=(s，v4，g).第一次生成的路径点集合包含冗余节点，通过可视性原则对路径集合P进行优化，删除冗余节点，得到二次筛选节点集合Popt，低层寻路算法按照二次筛选路径点集合Popt进行分阶段寻路.

图4 剖分点二次筛选

3.2 基于可视点的A*算法

在可视优化路径的基础上，采用A*算法依据可视点进行分段寻路，即对于机器人a由初始任务Task=(s，g)替换为分阶段寻路任务Taskopt=(Popt).需说明的是，在每个阶段，机器人在栅格地图采用A*算法进行四邻域搜索，即给定起点的情况下，向邻近无障碍栅格搜索，如图5 所示，机器人可以向上、左、右三个直线方向上的邻近空白区域拓展节点，但无法向下拓展节点，由此，避开障碍物.

图5 机器人节点拓展

综上所述，整个改进算法如图6所示，由地图处理、分阶段寻路、冲突检测三部分组成.在带约束的三角剖分地图基础上，进行图搜索和经过点的可视性优化，然后进行分阶段A*算法寻路.在CBS 算法框架下，算法不仅能快速找到最优路径，同时能够满足约束条件，最后生成一条无冲突的路径.

图6 LEADTOC算法流程

4 仿真结果

为测试所提出的LEADTOC 算法性能，本文采用文献［18］中给出的标准算例集，该算例集给出不同规模和复杂度的地图，以及机器人在地图中的起点和终点，由此进行的测试具有可复现性和可对比性.对比算法为考虑时空信息的标准A*算法［5］和在扩展度方面有优势的Tie-Breaking A*算法［14］，在本文中Tie-Breaking 策略取为：h=(1+d)×h，其中，h是启发式函数，为曼哈顿距离，d为Tie-Breaking 值，是一个很小的数字，本文A*算法、Tie-Breaking A*算法和LEADTOC 算法的d值分别取为0、0.001、0.001.所有算例结果均为20 次独立运行的平均值.测试采用的性能参数为：扩展节点数量，最终路径长度，求解时间（完成每次规划所需的时间）.所有运行均在matlab2020b环境下，11th Gen Intel（R）Core（TM）i5-1135G7@2.40 GHz和16 GB 内存容量的计算机配置下完成.

4.1 算法求解效率对比

4.1.1 简单测试算例

本算例为地图den312d下的测试，机器人起点坐标为（16，60），终点坐标为（55，37）.3 种算法的路径规划结果如图7 所示，其中红色线为最终规划的路径，蓝色区域为算法运行过程中扩展的节点.由图7 可见，本文提出的LEADTOC 算法和Tie-Breaking A*算法拓展节点数量都明显少于A*算法.3种算法的性能结果见表1.

表1 简单算例仿真结果

图7 简单算例3种算法节点拓展数量对比

由表1 可见，3 种算法规划的路径长度相同.在求解时间方面，本文算法高于Tie-Breaking A*算法.这是由于本文方法采用了三角剖分进行地图预处理和无障碍连通图规划，导致计算时间稍长于Tie-Breaking A*算法.但经过预处理后依然可以通过降低节点扩展量来减少求解时间，所以求解时间仍能比A*算法低.

4.1.2 复杂测试算例

图8 给出了复杂地图den520d 下的路径规划结果，其中，机器人起点坐标为（146，50），终点坐标为（10，183），起点和终点间的障碍物较为曲折复杂.表2 则给出各算法的性能指标结果.

图8 复杂算例3种算法节点拓展数量对比

表2 复杂算例仿真结果

由测试结果可见，在复杂地图上本文算法均可在拓展节点数量和计算时间上明显少于A*算法和Tie-Breaking A*算法.这说明本文算法采用的三角剖分优化虽然在地图预处理和无障碍连通图的生成方面有时间代价，但是当地图场景较为复杂时，该时间代价远低于节点扩展带来的时间消耗，故可较大提升算法的栅格路径规划效率.

4.2 更多测试算例

为了进一步验证本文算法的求解效率和求解质量，与A*算法和Tie-Breaking A*算法对比，对文献［18］中的10 个地图进行测试.图9 给出了若干典型地图样貌，由图可见不同复杂程度和复杂类型的障碍情况.表3 给出每个测试地图可用算例数量，可用算例数量指在MATLAB 笛卡尔坐标系下的可用算例数量，表4给出了10个算例下的算法性能结果.

表3 测试地图可用算例数量

图9 测试地图集

由表4 可得，LEADTOC 算法在节点拓展数量上为A*算法的5.3%～52.2%，路径长度为A*算法的98.1%～102.2%，平均求解时间为A*算法的44.3%.

表4 Benchmark测试结果

LEADTOC 算法在不规则复杂地形表现良好，在规则地形如Room 地形的求解质量稍差，LEADTOC 算法在大地形、复杂地形能发挥出改进算法的求解效率的优势.

下面是上述算例的结果分析说明：如表5 所示，以A*算法为基准，基准设置为1，分别对比了A*比A*、Tie-Breaking A*比A*、LEADTOC 比A*在节点拓展数量、路径长度、运行时间三方面的算法性能对比情况.由结果可得LEADTOC 算法在节点数量拓展对比，运行时间对比均低于另外2 种算法，在路径长度对比方面，长度基本持平.

4.3 动态冲突场景仿真测试

本节仿真实验地图为Benchmark 中的den520d，机器人和动态冲突的参数设置见表6.

如图10（a）所示，在不考虑动态冲突时，机器人在规划路径上将分别与动态冲突物1和2在冲突点相撞.对此，本文算法将冲突碰撞信息转化为约束传递给机器人，由机器人规划出一条无冲突路径，行进过程中可安全避开2 个动态冲突，成功到达终点，如图10（b）所示.可见，本文算法能很好地处理动态冲突物的碰撞信息，并规划出机器人的无冲突路径.

图10 动态冲突物仿真结果

5 结论

低层路径规划质量是影响多机器人路径规划的重要因素之一，对此，本文提出LEADTOC 算法，其中引入带障碍处理的三角剖分方法，并提出缩放因子以灵活地适应不同类型的地图且同时降低地图数据量，由此获得无固定障碍的路径引导.进一步采用可视化分段策略，令具有动态冲突处理能力的A*算法依相邻可视点进行分段路径规划，该策略可大大降低A*算法在路径探索时的节点拓展度.仿真结果表明，复杂地形下，LEADTOC 算法的节点拓展数量和存储空间需求量较低，在不降低求解质量的情况下能较好地提升求解效率.后续将对CBS 框架下多机器人的冲突判断和分解策略的优化展开进一步研究.