融合环境信息和运动约束的改进A*算法研究

白 雄，鲁吉林，路 宽，陈鹏云，崔俊杰，刘泽华

1.中北大学 机电工程学院，太原030051

2.中国人民解放军 第32381部队，北京100072

目前机器人技术得到广泛的发展，移动机器人在各行各业当中得到了广泛的使用[1]。其中，路径规划作为移动机器人自主导航的重要前提，在机器人领域扮演了重要的角色[2]。路径规划A*算法作为一种深度优先启发式搜索算法[3]，相比于图搜索算法，A*算法具有计算量小，算法搜索朝着目标点方向等优点，但是A*算法的启发函数简单，算法在寻路过程当中会出现较多的冗余点，花费时间长。针对这一问题已经有众多学者改进了A*算法。文献[4]提出一种基于改进的A*算法与动态窗口法结合的温室机器人路径规划算法，改进后的A*算法路径规划更为平滑和高效。文献[5]在A*算法的启发函数中加入余弦相似性和方法信息，选取36 阶领域矩阵解决了移动机器人贴合U型弯的问题，改进了路径曲率的变化。文献[6]在游戏地图中通过改进区域搜索点改进了A*算法，有效解决了路径转折问题。文献[7]针对公交线路提出一种改进A*算法，将乘客人数加入到估价函数中，提高了路径搜索的效率。文献[8]采用“视野线”以及“圆弧-直线-圆弧”改善了路径的平滑度。文献[9]将物理信息添加到启发函数当中并且删除冗余点，增强路径搜索效率和平滑度。文献[10]针对标准A*算法存在对称路径，使用定向搜索方法减少搜索点，提高了算法的效率。文献[11]通过引入三次均匀B样条插值函数，改进了A*算法，改进后的算法规划的路径更加平滑。文献[12]针对城市地图进行预处理，同时改进真实代价函数和估价函数对A*算法进行改进，改进后的算法能有效提高无人机的工作效率。文献[13]在全局路径的基础上选择关键节点对全局路径进行分段，然后结合局部路径算法进行优化，改善了路径的平滑度。文献[14]通过全局路径重规划解决了障碍物大面积阻碍全局路径的问题，提高了移动机器人避障安全性。上述文献可以看出，之前的改进算法主要局限于对代价函数和搜索点的改进，适用于简单的环境地图，算法并没有考虑移动机器人的运动特性、环境信息和避障能力。

为了解决以上问题，本文基于机器人运动特性和环境信息改进A*算法进行路径规划。首先通过环境地图中障碍物权重系数对代价函数进行改进；其次考虑机器人运动约束和最小安全距离，保证机器人运动的安全性。最后通过与其他经典算法进行对比分析，验证改进的算法在路径拐弯次数、扩展节点、规划时间和路径长度方面的有效性。

1 改进A＊算法

针对标准A*算法代价函数缺乏通用性和路径规划算法没有考虑机器人运动特性。改进A*算法首先通过环境地图信息设定启发函数；其次根据搜索方向计算子节点的代价值；最后结合关键节点和运动约束生成最优路径，具体流程如图1所示。

图1 改进A*算法的优化流程Fig.1 Improved A* algorithm optimization process

1.1 环境信息模型

移动机器人在进行路径规划之前要对环境地图进行合理的建模，由于栅格地图具有简单、高效的特点，则采用栅格法对环境信息建模，地图中黑色栅格表示障碍物栅格，在数组中表示为1；白色地图为机器人可通行栅格，在数组中表示为0，S为机器人人起始位置，G为机器人目标位置。如图2 所示。机器人最短路径就是通过搜索这张栅格地图来得到的。

图2 栅格地图Fig.2 Raster map

通过设置障碍物权重系数表达栅格地图的复杂度，对环境信息进行的分析，并且定义障碍物权重系数为当前地图中障碍物个数和整个栅格地图中的栅格个数占比。障碍栅格数为N个，移动机器人起始坐标为(xs,ys),终止坐标为(xg,yg),障碍物权重系数定义为：

1.2 启发函数的改进

标准A*算法规划出的路径存在较多的拐点和转折，不利于机器人运动。本文基于移动机器人运动特性和环境信息提出一种改进A*算法。

A*算法的评价函数由代价函数g(n)和启发函数h(n)组成，其中算法的最优搜索性取决于启发函数的选取，如式（2）、式（3）：

改进A*算法将环境地图的障碍物权重系数引入启发函数当中，如式（4）。在不同的环境地图当中，通过修改代价函数与启发函数的权重，提高算法的效率，保证移动机器人路径规划的最优性。

其中，g(n)为起始节点到当前节点的代价函数，h(n)为当前节点到目标节点的启发函数值，(xn,yn)为当前节点的坐标，a为代价函数g(n)的权重，即当前节点到目标点的距离与起始节点到目标点的距离比值。

A*算法中评价函数的好坏直接影响路径规划的效率。代价函数g(n)较大时，评价函数的值则会大于的实际代价，算法效率降低，从而导致机器人运动的实际代价过大，则算法将当前节点到目标点的距离与起始节点到目标点的距离比值设为代价函数的权重。通过式（4）可知，当障碍物权重系数较小时，增加启发函数的权重，A*算法减少搜索空间，提高了路径规划速度，减少路径的拐点和转折点；当障碍物权重系数较大时，减少启发函数的权重，增大搜索空间，避免算法陷入局部最优。因此，改进的评价函数可以在不同环境地图中提高路径搜索能力，有利于机器人找到最短路径。

1.3 搜索领域的选取

在搜索过程中A*算法向父节点的任意方向扩展，增加了算法的规划时间，改进的A*算法提出了一种新的扩展方式，即8 节点扩展方式改为5 节点扩展方式。定义终点坐标G(xG,yG)，当前节点坐标S(xS,yS)，通过坐标作差所得值与0进行比较来确定算法的扩展方向，如图3（a）所示。

xG-xS>0，且yG-yS>0，则可选节点为（1，2，3，6，9）；

xG-xS>0，且yG-yS<0，则可选节点为（3，6，7，8，9）；

xG-xS<0，且yG-yS>0，则可选节点为（1，2，3，4，7）；

xG-xS<0，且yG-yS<0，则可选节点为（1，4，7，8，9）。

通过确定节点扩展方向，然后从剩余的五个子节点选取下一个所要扩展的节点，每次选择评价函数值最小的子节点作为下一个路径规划的节点。

图3（b）显示父节点与其相邻的各个子节点的关系，主要将父节点的各子节点分为2类：父节点的上下左右四个节点和其余的四个节点。

图3 节点搜索方式Fig.3 Node search mode

针对标准的A*算法斜穿障碍物边缘可能性较大，增加子节点选取规则去避免此缺点，所用节点选取规则为：当障碍物节点为该父节点上下左右的四个子节点时，则四个子节点各自两边的子节点不作为扩展节点，如图4 所示1 号路径；当障碍物节点为其余的四个节点时，则不作任何处理，如图4所示2号路径。经过上述处理之后，改进A*算法可有效避免斜穿障碍物边缘。

图4 避开障碍物边缘后的路径Fig.4 Path after avoiding edge of obstacle

2 基于运动约束的路径生成

2.1 机器人运动学模型

移动机器人运动过程中，一般将移动机器人作为一个质点处理，而在实际中机器人具有一定的尺寸限制。机器人运动过程中是保持一定速度的，为防止机器人发生原地转向、侧滑等不安全动作，需对机器人路径进行优化。而移动机器人受到运动特性的限制，存在最小转弯角度，若机器人转弯时角度过大时，会导致机器人发生不安全的动作。考虑到移动机器人作为一个非完整约束系统，移动机器人的简化模型如图5所示。

图5 移动机器人运动模型Fig.5 Mobile robot motion model

某时刻下，机器人位置坐标为(x,y,θ)，其中x、y和θ分别是移动机器人质心位置的横纵坐标和机器人的航向角。机器人的导向角为ϕ（机器人中轴线和前轮行驶方向的夹角），R为机器人转向半径，l为前后轴距，b为轮距，v为当前速度。移动机器人的运动特性需满足如下约束：

由于机器人受运动特性所限，导致移动机器人的路径转角不能过大，否则将发生不可预测的不安全动作，改进A*算法将其作为路径规划的约束条件，使规划出的路径满足该运动约束。

2.2 Floyd算法路径平滑优化

平滑路径可使机器人更好地完成任务，提高机器人的运动精度。当前机器人路径的平滑方式有贝塞尔曲线和利用几何方式对移动机器人路径进行优化处理，但其复杂度高，难用在机器人频繁转向和转角过大的情况。文献[15]提出基于多项式的方法优化移动机器人路径，但该方法在复杂情况下多项式无解，不能保证算法的完整性。文献[16]在机器人路径规划参数单元中设定状态参数，依据状态参数生成贝塞尔曲线的控制点，优化了机器人路径，但是该方法存在着速度方面的限制，在运动过程中需不断校正参数。本研究融合移动机器人运动特性进行路径点的处理，通过Floyd 算法使得修正后的路径更加符合移动机器人的运动约束。

Floyd算法是解决地图中任意两点之间最短路径的一种经典算法。通过栅格地图规划出来的路径存在较多的转折点、搜索节点只能在栅格中心位置，导致机器人路径平滑度较差。本研究将Floyd 算法与A*算法相融合可以有效减少移动机器人路径的转折，提高机器人运动效率，有利于移动机器人的应用需求。考虑运动学模型判断两个路径点之间的距离、时间和转角是否比原来的短，算法优化路径对比如图6、图7所示。

图6 Floyd算法原理Fig.6 Principle of Floyd algorithm

图7 Floyd算法优化路径Fig.7 Floyd algorithm optimization path

如图6所示，节点A、D之间并没有可直达的路径，则在Floyd算法当中视为无穷大，那么有：

且

则，节点A到节点D的路径可表示为A、B、D。进一步有：

即：

则节点A到节点D的路径表示为A、C、D。

如图7所示，采用标准A*算法的规划路径为(S、S1、S2、S3、S4、S5、S6、S7、S8、G)，图中所示路径拐点及转折点较多，导致路径平滑度较差。

通过融合移动机器人运动特性的Floyd平滑算法剔除路径冗余节点，从而降低路径的转折次数以及优化路径长度，改善路径的平滑度。并通过判断两个节点之间是否有障碍物，以及两节点连线到障碍物的距离d和安全距离D的大小，来判断该路径是否可行，如图7 中的路径为S、S1、S7、G。

2.3 改进算法约束条件

文章改进的路径规划算法通过融合环境信息、搜索节点方式、安全距离和运动特性来进行全局路径规划，算法中所涉及的主要约束条件以及详细的数学描述如下：

（1）安全距离

改进A*算法设置路径节点到障碍物的安全距离，防止移动机器人和障碍物发生碰撞。障碍物到路径的垂直距离OE（点O到线段KG的垂直距离）与预先设置的安全距离进行比较来判断所规划的路径是否安全可行，安全距离如图8所示。假设节点K的坐标为(x1,y1)，终止节点G的坐标为(x2,y2)，障碍物节点O坐标为(x3,y3)；节点F的坐标为(x4,y4)线段OF为障碍物到线段KG沿纵轴方向的距离，记作l；KG之间连线与x轴之间的夹角为α，具体原理图如下：

图8 安全距离设计Fig.8 Safety distance design

其中，l为线段OF为障碍物节点到线段KG沿纵轴方向的距离，α为线段OE与线段OF之间的夹角，d为障碍物到路径的距离。

通过式（13）计算得障碍物到路径的距离d，设置安全距离为D。比较d与安全距离D的大小确定路径是否可作为备选路径。具体判断方式如下所示：

若d≤D，那么放弃该路径；

若d>D，那么选择该路径。

（2）机器人运动特性

首先设置移动机器人在移动过程中只能进行前后运动，当移动机器人路径角度发生改变时，调整移动机器人的姿态，然后按照已经规划好的路径移动到终点。

如图6 所示优化路径起点A(xa,ya)，节点B(xb,yb)和节点C(xc,yc)，移动机器人旋转角度计算方法为：

其中

β表示为机器人前一运动状态。并且移动机器人从A点到C点之间的路径为：改进A*算法主要融合上述约束条件对路径进行全局路径规划。

3 实验和分析

3.1 仿真环境

仿真实验主要环境为Intel®CoreTMi5-8400 CPU@2.80 GHz，Windows10。通过建立栅格地图来验证此次改进的A*算法的有效性以及改进A*算法对于复杂地图的适应性，同时将改进A*算法与标准A*算法、Dijkstra 算法以及蚁群算法进行对比，检验改进A*算法的优越性。仿真实验所用的栅格地图为4 个尺寸为50×50，障碍物权重占比不同的栅格地图。地图中的单位栅格边长为1 m，栅格地图中空白区域为可通行区域，黑色区域为不可通行区域，仿真起点为地图左下角顶点，终点为地图右上角顶点。安全距离D设为0.8 m。

3.2 仿真结果与分析

改进A*算法的约束条件可分别计算路径长度和路径转角，如式（13）、（14）所示；搜索节点数通过节点选取规则加入Open 列表中节点个数可知，通过上述评价指标对仿真实验进行评价。通过50×50 的栅格地图对蚁群算法、标准A*算法、Dijkstra 算法以及改进A*算法在Matlab中进行测试，测试结果如图9～12所示。

图9 地图1仿真结果Fig.9 Simulation results of map 1

上述四个栅格地图障碍物权重占比逐渐增大，环境地图越来越复杂，地图主要包含了排列整齐的障碍物以及排列混乱的障碍物，所利用的环境地图有较好的代表性。仿真实验通过在Matlab中对比改进A*算法与标准A*算法、Dijkstra 算法以及蚁群算法得到路径规划的时间、转折次数、转角度数、搜索节点个数以及路径长度等，如表1所示。

表1 不同地图下几种算法对比实验Table 1 Comparison tests of several algorithms under different maps

图10 地图2仿真结果Fig.10 Simulation results of map 2

图11 地图3仿真结果Fig.11 Simulation results of map 3

图12 地图4仿真结果Fig.12 Simulation results of map 4

由图9～12 所示的仿真结果可知，在所使用的四种环境地图中，地图复杂度由易到难，障碍物所占比重逐渐增加。改进A*算法和其余三种对比算法都可以规划出路径，但是改进算法和其余路径规划算法在转折点、规划时间、路径角度、搜索节点以及路径长度上有较大的不同。在不同环境地图中，改进A*算法规划出来的路径更有利于机器人运动，改进算法主要在机器人运动特性的基础上通过Floyd算法进行处理路径，Floyd算法是解决地图中任意两点之间最短路径的一种经典算法，在两点之间无障碍物时，删除冗余点进行规划路径，所以改进A*算法规划路径与搜索节点并不能一一对应，在路径长度以及路径弯曲程度有很大的改进。

从表中数据可以看出：标准A*算法是经典的启发式算法，规划出来的路径通往目标点所在的方向，并没有考虑障碍物的影响，而是选取了局部最短路径；蚁群算法规划出来的路径由于信息素的影响，路径转角过大，并且路径规划时间长，算法在1 500 代左右达到收敛，并没有找到最优路径；Dijkstra 算法简单，能够得到最优解，但是其搜索节点过多，效率比较低，占用过多内存，路径搜索时间长等缺点导致它并不适合复杂地图；在复杂地图当中改进A*算法相比其他算法有较大的优势，相比于以上四种算法，此次改进的算法路径转折点更少，规划出来的路径转角降低了82%，更适合机器人运动；并且始终与障碍物保持一定的距离，避免移动机器人与障碍物发生碰撞；路径规划时间平均减少了80%；路径长度在不同地图中相比其他算法都有很大的改进。上述分析可知，改进后的A*算法在移动机器人路径规划当中会起到更好的效用。

4 结束语

基于运动特性的改进A*算法，针对标准A*算法在路径规划过程中拐点较多、转角大不利于机器人运动、路径规划时间长以及搜索路线紧贴障碍物边缘等缺点，导致移动机器人容易发生不安全动作，不利于机器人的行进，提出一种改进A*算法。主要结论如下：

（1）通过环境地图中障碍物权重系数对代价函数进行改进，算法搜索节点更具有方向性，提高了算法的效率，也保证了算法的实时性。

（2）在路径优化过程中考虑机器人运动特性和安全距离等约束条件，保证移动机器人运动安全性。

（3）实验结果表明，改进后的A*算法在路径规划中始终与障碍物保持一定的距离，搜索的节点和路径规划时间减少；融合运动特性的路径规划更为平滑，路径转角更小，更有利于移动机器人的运动。