基于地图预处理及改进A*算法的路径规划①

余文凯* ** 章 政 付雪画 王昭伟

(*武汉科技大学机器人与智能系统研究院 武汉 430081) (**武汉科技大学信息科学与工程学院 武汉 430081) (***中山大学数学学院 广州 510970)

0 引 言

路径规划是实现移动机器人自主无碰撞移动的关键技术之一。机器人根据特定的工作需求，依据路径距离最短、能耗最少或者移动时间最少等优化指标，搜索一条从起始点到目标点的最优路径[1-4]。A*算法具有搜索效率高、规划速度快和克服了搜索过程中形成的早熟现象等特点，在机器人路径规划中得到了广泛应用[5，6]。

传统A*算法在路径规划应用中，随着地图面积的增大其搜索空间会产生多余的搜索节点，导致算法效率降低、规划路径出现不必要的转折等问题。针对这些问题，许多学者从不同的角度提高了A*算法的搜索效率。文献[7]在启发函数中增加了父节点信息，并修改了评价函数的权重，增加启发函数的权重，减少搜索空间来提高算法的搜索效率。文献[8]引入领域矩阵进行障碍搜索以提高路径的安全性，并通过结合方向角度和位置距离对启发函数进行修改，提高了计算效率。文献[9]加权处理评价函数，并通过人工搜索标记，减少了搜索区域，让搜索的精度下降，来提高算法的效率。文献[10]针对复杂地形的路径规划问题，在A*算法的评价函数中增加安全成本，并将评价函数转换成统一的时间成本，由此提高了机器人的安全性。然而，路径规划问题不仅与路径搜索算法有关，还与环境地图复杂度密切相关。经研究发现，路径规划搜索算法的效率与地图搜索空间的规模成反比，如A*算法的时间复杂度为O(n2)(n为节点数)。因此，通过缩减搜索空间的规模是提高A*算法效率的有效途径之一。

基于上述分析，本文将局部环境信息识别与A*算法相结合，提出了一种基于地图分区预处理及改进A*算法的路径规划算法。一方面，通过对A*算法本身进行改进来提高搜索效率；另一方面，通过预先分析环境地图，依据环境复杂度改变搜索空间规模来提高搜索精度。首先，基于栅格法对地图建模，引入K-Means聚类算法对地图栅格模型进行分区预处理，根据障碍点数量确定地图区域的规模，并依据障碍物的数量和位置判断每个区域环境的复杂度。然后，依据地图聚类结果，改进A*算法的评价函数和子节点选择方式在不同区域内进行灵活的搜索。最后，改进Floyd算法对路径进行防碰撞平滑度双向优化处理。实验结果表明，本文所设计的路径规划优化算法具有适应性强、搜索效率高、路径转折次数少和平滑度高等特点。

1 地图分区预处理

在采用栅格法构建地图的模型中，对障碍栅格点的数量及位置信息进行分析，量化环境的复杂度，并将环境复杂度信息加入到A*算法的搜索方式中，可有效提高算法的搜索效率。然而，全局地图的复杂度信息不能有效地描述局部环境的复杂度，需要识别地图的局部信息。在障碍率高的局部区域，需扩大搜索空间，选择穿过高复杂度区域的最小代价路径，在障碍率低的局部区域减少搜索空间，需快速搜索最短路径。

因此，本文采用栅格法建立地图模型后，用K-Means聚类算法根据障碍物数量和位置进行分区预处理，判断每个区域的复杂度，在每个区域内进行不同精度的搜索，提高算法局部搜索效率和灵活性，获得全局最优路径。

1.1 栅格法地图建模

基于栅格法对移动机器人工作环境进行地图建模。设机器人工作区域为m×n，单元栅格边长为l，则2维建模地图大小为xmax×ymax。

(1)

其中，ceil为进一取整函数，每个栅格都有对应的坐标表示，并映射一个值(1或0)。当该栅格内有障碍物时赋值为1，用黑色表示；无障碍物时赋值为0，用白色表示。路径规划时，只需遍历值为0的栅格。

1.2 地图环境分区预处理

1.2.1 基于K-Means的分区处理

K-Means聚类算法通过对象与聚类中心之间的距离进行聚类，其目标通常是将距离比较相近的对象组成簇，从而得到紧凑而独立的不同簇。此外，K-Means聚类算法简单、收敛速度快，具有可伸缩性和高效性。本文利用K-Means聚类算法对环境地图进行聚类，可完成对环境地图的完全分割，避免剩下多余区域。同时，在对地图分区预处理中，将2维栅格地图中的每个障碍栅格点当作聚类样本，K-Means聚类算法依据这些样本的位置坐标进行聚类处理。地图信息处理时，K-Means算法通过预先设定的K值(即聚类簇群数目)进行处理。K值的设定由地图规模而定，设置过大会导致路径局部最优，设置过小则无法有效量化环境信息[11，12]。

1.2.2 量化环境信息

通过K-Means聚类算法预处理后，可分别提取每个族群的环境信息。为量化地图的信息，设计障碍率Po和直通率Pt。

(2)

(3)

其中，no是值为1的栅格(障碍栅格)的数目，N是族群内栅格总数，nt是一行或一列值都为0的栅格(无障碍栅格)条数，L和D是该族群的栅格行数和列数。

障碍率Po描述了环境中障碍物数量的多少，Po越大，障碍物越多。直通率Pt描述了环境中障碍物位置的混乱程度，Pt越大，混乱度越小。通过障碍率Po和直通率Pt描述区域环境中障碍物的复杂情况，其数值大小影响改进A*算法的评价函数，使得搜索空间随地图环境变化而改变，在不同的环境中有更加有效的搜索方式，增加了A*算法的灵活性，提高搜索效率。

1.2.3 地图分区处理算法步骤

基于K-Means地图分区处理的算法步骤如下：

步骤1输入障碍栅格点样本D={x1,x2, …,xm}，从数据D中随机选择k个质心向量{u1,u2, …,uk}。

步骤2设置最大迭代次数N。

(1) 将簇划分k类，并初始化Ct(t=1, 2,…,k)为空集；

(4) 如果所有的k个质心向量{u1,u2,…,uk}都没有发生变化，转到步骤(3)；否则，转到步骤(2)重新计算。

步骤3输出簇划分C={C1,C2,…,Ck}。

步骤4由式(2)和式(3)计算k类簇群{C1,C2,…,Ck}区域内的障碍率Po_i和直通率Pt_i(i=1,2,…,k)。

供应链上的一些相关事务，例如计划、采购、生产、物流等流程，大多数都是通过权威机构进行的“中心化”的设计与管理。这些权威机构就如平时生活中的银行、支付宝，被人们所信任，但是如果出现内部人员的恶意操作，导致的后果也是不可估计的。

步骤5改进A*算法在Ct(t=1,2,…,k)区域搜索时，输出该区域的障碍率Po和直通率Pt到评价函数f(n)中。

2 融合环境信息的改进A*算法

本文基于K-Means聚类算法对地图进行分区预处理后，设计了A*算法的评价函数f(n)，将量化环境信息的数据加入评价函数中。然后，改进子节点选择方式，避免路径斜穿过障碍物顶点。最后，改进Floyd算法对规划后的路径进行双向平滑度优化处理，并设置安全距离防碰撞。

2.1 评价函数设计

A*算法是解决静态路网中最短路径的最有效的直接搜索方法，也是解决许多搜索问题的有效算法[3]。算法中距离估算值与实际值越接近，最终搜索速度越快。其评价函数为

f(n)=G(n)+h(n)

(4)

其中，f(n)为评价函数，G(n)为起始点到当前节点n实际路径代价的代价函数，h(n)为当前节点n到目标点估计路径代价的启发函数。本文的评价函数中引入了环境信息和机器人位置信息，并设置了代价函数和估价函数的权重函数。

(5)

(6)

f(n)=r1G(n)+r2h(n)

(7)

其中，r1=Pt+dsn/dst，r2=(1-Po)+ednt/dst，dsn表示起始点到当前点的距离，dnt表示当前点到目标点的距离，dst表示起始点到目标点的距离。

将预处理中式(2)和式(3)得到的障碍率Po和直通率Pt数据代入式(7)中，评价函数f(n)直接影响了路径规划的好坏和效率。代价函数G(n)越大，评价函数f(n)就会越大于实际的代价，导致算法效率降低，路径距离变长，从而使移动机器人运动代价过高。启发函数h(n)的权重越大，算法的运算速度就越快，但路径会更加趋近代价最小路径，陷入局部最优，造成转弯次数较多的路线。

将环境信息和移动机器人位置信息都融入到评价函数中。在不同的区域内，权重函数进行自适应调整，产生不同的有效搜索空间，不仅保证算法的快速性和灵活性，还保证路径的全局最优。

2.2 优化子节点的选择

传统的A*算法中，判断下一个子节点时，选择条件只考虑了子节点是否存在障碍物，没有考虑子节点与障碍物的位置关系。所以，规划出的路径斜穿过障碍物的顶点，存在发生碰撞的风险。针对此问题，本文增加了子节点选择规则，避免路径斜穿过障碍物顶点。

根据8个子节点与父节点的坐标关系，如图1子节点图。将在父节点的上下2个子节点(子节点2,6)分为Ⅰ类，将在父节点的左右2个子节点(子节点4,8)分为Ⅱ类，进行优化子节点的选择。

(1) 如果障碍物为Ⅰ类，删除该障碍点的左右2个可选子节点(如子节点1,3或者子节点5,7)。

(2) 如果障碍物为Ⅱ类，删除该障碍点的上下2个可选子节点(如子节点1,7或者子节点3,5)。

(3) 如果障碍点不是I类和Ⅱ类，则不做处理。

图1 子节点图

改进后，规划后的路径不再穿过障碍物的顶点，避免移动机器人在运动中与障碍物发生碰撞。

2.3 基于改进Floyd算法优化路径平滑度

Floyd算法[13]又称为插点法，是一种利用动态规划的思想寻找给定的加权图中多源点之间最短路径的算法。基于栅格法地图建模所规划出的路径，存在多次转折、路径节点只能在栅格中心的问题，导致路径不是最短且平滑度较差。针对这些问题，依据Floyd算法，设计了双向优化处理过程，常规Floyd算法从起始点到目标点正方向进行平滑度优化，本文改进Floyd算法，增加从目标点到起始点的反方向优化，实现双向平滑度优化，可有效减少路径长度，并增加安全距离判断，避免与障碍物发生碰撞。

在改进Floyd算法双向优化中，增加防碰撞隔离距离，通过障碍点到连线的垂直距离与设置的安全距离的关系，判断路径是否安全，实现防碰撞。障碍隔离关系如图2所示，路径节点A的坐标为(xa,ya)，B的坐标为(xb,yb)，障碍点C的坐标为(xc,yc)，隔离距离为点C与节点AB之间线段的垂直距离d，到节点AB之间线段纵轴的距离CD为l，节点AB之间线段与横轴之间的夹角为∂。

由A、B及C 3点坐标可求出障碍点到路径的距离为

d=lcos∂

(8)

图2 障碍隔离图

改进Floyd算法将规划出的路径节点进行平滑度优化，增加从目标点到起始点方向的优化，进行双向平滑度优化，设置Floyd算法中取点步长k的数值，并设置安全距离D的参数大小。以图3中所示的路径为例说明双向优化路径平滑度步骤，图中S为起始点，G为目标点。

图3 路径平滑优化例图

步骤1删除路径节点同一线上的中间节点，只保留起点、拐点和终点。如图3所示，处理后路径为S→n1→n2→n3→G。

步骤2从起始点S方向，在保留的2路径节点之间每隔k步长取点，每取一个节点与前一端路径节点，判断2点之间路径是否有障碍物，并由式(8)计算路径旁边障碍物到路径的距离d，判断与安全距离D的大小。若障碍物在安全距离之外，满足要求则选择当前节点为路径节点，否则不选择。如图3所示，处理后路径为S→n22→G。

步骤3从目标点G方向，反向取点判断(重复步骤2的判断方式)，如图3所示，处理后路径为S→n21→G。

依据改进的Floyd算法对规划后的路径进行双向优化，并增加防碰撞隔离，使得路径节点选择不再局限于栅格中心点，减少了转折度数和路径长度，减少了运动代价，增加了路径的平滑度。

3 仿真实验及结果分析

首先建立30×30的移动机器人工作环境地图，将本文算法与地图未预处理的改进A*算法和传统A*算法进行对比仿真测试，验证基于K-Means地图分区预处理及改进A*算法的路径规划算法优点。再扩大地图规模，建立50×50的环境地图模型，将本文算法与传统A*算法对比仿真测试，验证本文算法的优点及灵活性。

为方便对比研究，假定移动机器人工作环境栅格边长为单位1 m，设安全距离D=0.8 m，取点步长k=0.1 m。地图中S表示起始点，T表示目标点。黑色区域为障碍物，灰色区域表示该算法的搜索空间。在基于K-Means地图分区预处理及改进A*算法中，以不同形状的符号表示各个区域的障碍物，区别不同的分区，以圆圈符号表示基于K-Means聚类处理出该区域的中心质点。

同时，为综合评价算法的性能，设定了评判函数T，T是机器人按算法规划的路径进行移动所需要消耗的时间。

T=L/v+N+θ/45

(9)

其中，L表示规划路径的长度，v表示移动机器人运动速度，假设速度为1 m/s，N表示转弯次数，θ表示转角总度数，假设每次转弯消耗加减速的时间为1 s，每转45 °消耗的时间为1 s。

3.1 实验1

参考自动导引小车和其他室内移动机器人的工作环境，设计了30×30的仿真环境地图。基于传统A*算法的路径规划如图4所示。基于改进A*算法，在未分区处理的地图及基于K-Means预处理的地图环境中(聚类簇群数目K=4)，路径规划分别如图5和图6所示。其中，对于未分区处理的算法中直接计算整个地图环境的障碍率和直通率。3种算法的实验路径参数如表1所示。

由表1的实验数据可知，传统A*算法搜索空间数量为359个，远大于其他2种算法，改进后A*算法效率明显提高。未作地图预处理的改进A*算法没能有效识别局部信息，陷入了局部最优解，导致路径频繁转弯，消耗了机器人的移动时间。而地图分区预处理的改进A*算法对局部复杂区域扩大了搜索空间，转弯次数减少了46%，转角总和减少了49%。实验表明，结合了K-Means地图分区预处理的改进A*算法灵活性更高，能依据环境信息的不同，有效改变搜索空间，获得全局上的最优路径。在长度差不多的情况下，转角次数减少，转角总和减少，提高了路径的平滑度。

图4 传统A*算法规划路径

图5 未分区处理改进A*算法规划路径

图6 基于K-Means分区预处理改进A*算法规划路径

表1 实验路径参数表

在此地图内进行5次仿真测试，选择不同起始点和不同目标点进行实验。由式(9)计算5组实验的消耗时间，对比移动机器人消耗时间T，如图7所示。

通过5次仿真测试，由图7中数据可得，基于K-Means地图分区域的改进A*算法比其他2种算法所消耗移动时间T都少。在实际应用中，减少移动时间，提高了移动机器人的工作效率。

图7 路径消耗时间T值对比图

3.2 实验2

为了进一步验证本文算法的搜索效率和环境适应性，将地图规模由30×30扩大为50×50，环境随机产生障碍物。在基于K-Means聚类地图预处理算法中，K=5。基于传统A*算法和本文所设计算法的路径搜索如图8和图9所示。

从规划出的路径图8和图9可知，本文算法搜索空间减少，搜索效率提高，规划出的路径始终与障碍物保持安全距离，避免了移动机器人与障碍物发生碰撞的危险。由表2路径数据表可知，本算法规划出的路径长度和转角次数少于传统A*算法规划出的路径，转角总和比传统A*算法减少63.3%，平滑度提高。在算法的效率上，改进A*算法搜索空间数减少了54.8%，算法的搜索效率也得到了提高，减少了消耗的移动时间T，提高了移动机器人的工作效率。

图8 传统A*算法规划路径

图9 基于K-Means分区预处理改进A*算法规划路径

表2 实验路径参数表

实验表明，本文算法比传统的A*算法不仅减少了搜索空间，提高了算法的搜索效率，还提高了路径的平滑度和安全性。

4 结 论

本文针对地图外部的环境因素及A*算法内部的运算因素对移动机器人路径规划搜索效率的影响问题，设计了一种基于地图分区预处理及改进A*算法的路径规划。基于K-Means算法对地图进行聚类分区预处理，并以此对A*算法的评价函数和子节点选择方式进行改进，能在不同区域依据环境信息采用不同搜索方式，并改进Floyd算法进行双向平滑度优化，防碰撞处理。仿真结果表明，本文算法在根据地图环境进行有效构建路径的搜索空间基础上，改进A*算法的搜索机制和灵活度，从而保证路径规划效率。地图分区预处理可以提前对地图分析并量化环境的复杂度，平均有效减少54.8%的搜索空间，提高了算法搜索效率、精度和灵活性，规划出的路径始终与障碍物保持安全距离，避免发生碰撞，而且转折度数减少了63.3%，平滑度得到了较大提升。路径消耗机器人的移动时间更少，提高了移动机器人的工作效率。