一种对地图特征定向采样的改进A*算法

赵梓辛，肖世德，靳天石，杨明亮，2

（1.西南交通大学机械工程学院，四川 成都 610031；2.先进驱动节能技术教育部工程研究中心，四川 成都 610031）

1 引言

路径规划技术在日常生活、高新技术、决策管理等诸多领域具有广泛的应用，主要解决成本、效率、安全等问题［1-2］。

移动机器人路径规划是指在一定的环境模型基础上，给定移动机器人起始点和目标点后，按照算法规划出一条无碰撞、能安全到达目标点的有效路径［3-4］。近年来新的路径规划算法不断出现和发展，在移动机器人路径规划领域具有代表性的是基于采样的算法和基于搜索的算法［5］。基于采样的算法是通过均匀随机采样，将环境空间离散的点构建成连通图，效率高，但所得的路径代价高。基于搜索的算法是将环境空间离散成栅格的形式，然后利用代价函数最小搜索可行解甚至最优解，但在大量的搜索计算下实时性很难满足要求。路径规划效果的关键在于规划算法的性能，而规划算法评价指标有两个：一个是路径代价，描述路径长度和冗余拐点；另一个是算法实时性，描述算法所需的时间。

国外对于相关研究较为深入，例如Weighted A*［6］通过增加启发式函数的权重，进一步加速向目标节点的搜索，但容易陷入局部最优，所得解大概率为次优解；ARA*［7］是在规定时间内多次执行Weighted A*，并且每次执行时将启发式函数的权重逐渐减小，使次优解不断逼近最优解，但是要多次执行Weighted A*，运行时间也会成倍增长，所以在允许时间内一般为次优解；还有Sandip Aline等提出了MHA*［8］，引入多个启发式函数，保证其中有一个启发式函数在单独使用时可以找到最优解，从而通过协调不同启发式函数生成的路径代价，可以兼顾算法的效率和最优性，但是确保一个最优启发式的过程和协调的过程是一个耗时的前处理。国内研究也在跟进，例如利用双向A*算法［9］双向同时使用A*算法，从而提高了算法的执行效率，但是路径并未得到优化。

上述搜索路径规划方法多数从启发式函数角度优化，其根本目的就是减少无关节点的搜索，并保证搜索到关键节点，从而保证路径最短的同时避免处理大量节点。但是在减少节点的过程中，节点减少过少时，势必还是有很多无关节点计算；节点减少较多时，可能忽略关键节点，致使路径偏离最优解。再者，不同的环境地图，所具有的特征是不相同的，所匹配的最优启发函数也是不一样的。因此，与其优化启发函数甚至多启发函数混合，不如从环境地图特征入手，用这些特征点代替原地图，从而降低搜索节点的数量。故源于采样的路径规划思想，对环境地图对地图特征做定向采样处理。进而在地图特征点构建的无向路径网络图上，使用基于搜索的算法找出最优解。这样既避免了基于采样算法的随机性带来的路径成本过高问题，又避免了基于搜索算法在处理大量节点时效率低下的问题，从而达到降低路径成本并提高算法实时性的目的。

2 改进A*算法的实现

2.1 环境模型描述

对移动机器人周围环境进行数学模型搭建是使用算法规划路径的前提。均匀栅格地图［3］是移动机器人规划中最常用的表现形式。最早是由Morave和Elfes提出的，其基本思想是将有限地图空间离散分解为多个大小相同并以数值表示状态的栅格单元［10］。由此方法将环境空间分解为大小统一的M*N个栅格，它们均匀分布组合成二维地图信息U，其中每个栅格的状态信息由Mapij表示：

其中，每个栅格单元的状态信息Mapij取值如下：

式中：Mapij=0—移动机器人可通行的自由区域；Mapij=1—移动机器人不可通行的障碍区域；Mapij=2—移动机器人的起始位置；Mapij=3—移动机器人的目的位置。

为简化研究，做出以下假设：

假设1 以均匀栅格地图表示实际环境，只考虑二维空间信息，忽略高度信息。

假设2 移动机器需考虑占据的二维空间，忽略高度信息。

假设3 移动机器每次移动，其几何中心占据一个栅格。

假设4 此全局规划算法，是为移动机器人运行做方向指引，移动机器人的实时路径应由局部规划实现。因此假设移动机器人前进和转向行为分开单独进行。

2.2 地图特征提取

最优的路径一般出现在障碍物的边界处。若不存在障碍物，起始点与目的点可以通过一条直线达到，也正是障碍物的出现，才出现了路径规划的问题，因此障碍物外部轮廓可作为地图特征。边缘角点是障碍物轮廓的连接点，并可通过角点的连通关来表示障碍物的轮廓。因此可以将角点和角点的通断信息来作为地图特征。

边缘角点本来是一种图像特征。边缘角点检测是图像处理中一种方法，是对二维数组的数字图像进行的处理方式。在2.1节中，环境模型也是由一个二维数组描述的，图像中的每个元素称为像素，用于存储灰度值，而环境地图中每一个元素称为栅格单元，用于存放状态值。均匀栅格地图和数字图像的本质相同，是将角点检测方法应用到路径规划的必要前提。

此处角点检测并不是完全应用图像处理的方法，状态空间的地图的处理与图像的处理还是有一定区别，状态空间地图只有一个通道，因此不需要灰度处理，本身通道可理解为灰度通道，而且通道只需处理0或者1值，相比于彩色RGB通道图片的角点处理计算量非常小。具体实现步骤如下所示。

（1）分别计算地图的水平、垂直的状态值变化量矩阵Ii、Ij。采用一阶横向梯度滤波算子di、一阶纵向梯度滤波算子dj对原地图矩阵U滤波得矩阵Ii、Ij。

（4）计算地图矩阵U各个像素点的角点量，构建角点量矩阵R，此处k取0.04。

式中：R—角点量矩阵。

当R为正值时，此栅格单元为角点；当R为负时，此栅格单元为边；当R很小时，此栅格单元为平缓区域。

（5）关键角点的筛选。

①为了降低无关角点的数量和相邻角点数量，同时找出最关键的角点。以全局最大角点量值Rmax设置阈值thresh，式th中取0.3。

并求以5*5窗口遍历并求窗口局部最大角点量值，并通过阈值抑制的局部角点即为全局角点。

②在此路径规划中，角点作为路径途径的候选点，如果其出现在障碍物内部，会导致碰撞的严重后果。但是图像处理中，边缘角点必然出现在障碍物内部边缘。对于这种问题，采用膨胀障碍物的方式，让角点从障碍物区域移至障碍物膨胀区域，从而避免事故的发生。

③为了剔除地图边界的无关角点，以及出现在非障碍区域的错误角点选出的角点再进行一次是否在障碍膨胀区域的判定，在膨胀区域的才为全局角点。

为了方便直观描述，膨胀区域不显示，如图1所示。左下角红色圆点为目的点，右上角红色圆点为初始点，障碍物周围的红色星点为筛选出的角点。

图1 地图特征提取结果Fig.1 Map Feature Extraction Results

2.3 障碍物膨胀处理

大多数路径规划的研究中，常常将移动机器人假设为点进行研究，忽略其占据的空间，可能出现移动机器人边缘与障碍物发生碰撞的危险行为。因此为了避免发生上述情况，将移动机器人在水平面上占据空间的大小考虑到规划算法中。移动机器人在搜索中是动态的角色，如果时刻考虑质心和障碍物的距离，再和移动机器人的长宽做判断是否发生碰撞。每一次扩张都会计算并判断一次是否碰撞，加剧了算法的空间复杂度。以移动机器人质心到边缘最大距离为尺度在障碍物上做一次性的膨胀处理。这样相比与实时计算是否碰撞，降低了很大的计算量。

障碍物的膨胀处理的伪代码如下：

在上述伪代码中，Restricted_area（Pn，τ+1）表示膨胀当前点Pn的τ+1层邻近栅格，τ由移动机器人质心到边缘的最大距离决定，τ+1表示添加一层角点检测的膨胀区域。在膨胀的过程中，将位于自由区域的栅格，设置为障碍物膨胀区域。如下图所示，数字5的区域是障碍物区域，并以1层邻近栅格作为膨胀，将1、2、3、4、6、7、8、9自由区域膨胀为障碍物膨胀区域。

图2 障碍物膨胀处理原理Fig.2 Principle of Barrier Expansion Treatment

2.4 构建无向路径网络图

将得到特征角点与始末点两两相互连接，摒弃与障碍物或膨胀区域有交集的连线，保留无碰撞连线，构建出无向路径网络图。如下图所示，此图是初始点v1，目的点v8与正方形障碍物的4个角点，三角形障碍物的3个角点构成了基于地图特征的无碰撞无向路径网络图，从而将100*100的地图转化为在9点之间连通网络图，降低搜索成本，提高搜索效率。

图3 无向路径网络图Fig.3 Path Network Diagram without Direction

但是无向路径图不方便存储也不方便搜索，先将其以邻接矩阵的形式存储。无向路径图中的信息分为两种，一是各个顶点的信息，二是各个顶点的通断关系。用邻接矩阵E存放顶点的关系信息。但各个顶点的信息部分无法存储在E中。因此，再添加数组V存放图中所有顶点数据。两个数组共同存放无向路径图所有信息，如式（11）、式（12）所示。

式中：矩阵E—描述顶点关系的矩阵。元素E（i，j）—V（i）和V（j）顶点的关系。顶点信息V与顶点关系E具有以下性质。

（1）顺序存储对应性。对于顶点在V数组的存储顺序与二维数组E相对，即E（i，j）表示顶点V（i）与顶点V（j）的关系。

（2）主对角线元素都为0。主对角线表示V（i）与V（i）的关系，此时，E（i，j）取0。

（3）其余任意元素的意义，如下式所示。

（4）对称性。邻接矩阵是关于主对角线对称。对于E（i，j）与E（j，i）分别表示V（i）到V（j），V（j）到V（i）连通性，因为连通图没有方向性，因此E（i，j）与E（j，i）是相等。

2.5 基于邻接矩阵的改进A＊算法

基于搜索的路径算法中，A*是在经典Dijkstra算法的框架下引入估价函数，优先搜索代价低的栅格。估价函数表示式为：

式中：F（v）—起始节点经由当前区域到目的位置的总代价值；G（v）—起始位置到当前节点v的实际代价；H（v）当前节点v到目的位置的代价估算值；H（v）—般取欧几里得距离，定义如下：

式中：（xv，yv）—当前节点v坐标；（xgoal，ygoal）—目标节点坐标。

传统A*算法维护两个集合：OPEN和CLOSED。CLOSED存储已经搜索过的节点。OPEN 存储已搜索过节点的子节点优先队列，根据路径总代价值F（v）由小到大排序。算法每次从OPEN中取出总代价最小的节点，作为新的搜索节点插入到CLOSED中，再将该节点进行扩展，最后将不在CLOSED集合中的扩展节点，插入到OPEN中。这样一次一次循环下去，直到扩展到目标节点。其中扩展的意义是在栅格地图的前提下，对当前节点周围节点进行遍历。最常见的扩展方式如图4（a）、图4（b）两种扩展方式。当前点以“米”字向周围栅格进行扩展，一共8个方向的最近栅格进行遍历，因此也称作“八连接”扩展方式，如图4（a）所示。当前点以“十”字向周围栅格进行扩展，一共4个方向的最近栅格进行遍历，因此也称作“四连接”扩展方式，如图4（b）所示。

图4 “八连接”“、四连接”扩展方式示意图Fig.4 Schematic Diagram of“Eight-Connection”and“Four-Connection”Expansion Modes

其中对于每一步扩展的代价式（16）进行计算，式中：v’—当前节点v的扩展节点。

改进A*算法的实现如下伪代码所示，加粗字体区域为在A*算法基础上所作的改进。伪代码分为两个部分，初始化部分和路径搜索部分。1-2行为初始化部分，和传统A*一样的初始方法。除去G（vstart）取0外，所有其他点的实际代价值G 都取无穷大。OPEN中只存放vstar节点，并且CLOSED为空。

3-17行为路径搜索部分，3行伪代码SearchPath（），开始运行路径搜索程序。

4行将传统while循环条件goal is not expanded改进为OPEN≠∅。传统A*算法中由于估计函数H（v）非最优，在扩展到目标点时，算法就会结束，此时的路径长度可能会大于最优解，从而得到次优解。若OPEN为空时算法结束，此时算法扩展到目标点搜索到可行路径后，算法会继续搜索，直到OPEN为空，在这期间会搜索到多个可行路径，从中找出最优路径。

6行、8行、11行出现的solution是用来存放次优路径。在搜索算法执行到OPEN为空的过程中，为了降低可行路径的存储空间。在第二个可行路径出现时，与前一个存放在solution中可行路径做对比，将较优路径在11行中再次存放在solution中。同时为了减少可行路径的数量，6行当前在OPEN中的代价最小的节点必须满足F（v）＜F（solution），8行在当前节点的扩展子节点代价总和满足G（v）+C（v，v’）+H（v’）＜F（solution），从此避免计算过多节点，避免生成过多的次于当前路径的可行路径，浪费计算资源，降低实时性。

8行中StateExpand（）表示执行状态扩展。传统状态扩展采用“八连接”扩展方式，这种方式让路径只能一格一格前进，导致所得路径并非最优。针对这一问题，基于邻接矩阵的扩展方式，去除大量中间点，让关键点连接，在其上的扩展。结合式（11）、式（12）对邻接矩阵的定义，StateExpand（）程序的伪代码如下所示。

3 实验过程及结果评价

为了验证改进的A*算法的性能和适用范围，在Matlab R2019a仿真平台中，构建多障碍物的栅格环境下进行了仿真。

首先，构建如图5（a）所示的栅格地图，尺寸为100×100。其中，黑色栅格表示障碍物区域；白色栅格表示无障碍物区域；左上方和右下方的两处黑色圆分别表示坐标为（5，5）的起始点、坐标为（95，80）的目标点。并在此栅格地图上，运行传统A*路径规划算法。由图5（b）可得传统A*算法规划的路径有几处明显与障碍物相交。

图5 栅格图构建、传统A*仿真结果示意图Fig.5 Raster Diagram Construction and Traditional A* Simulation Results

再对传统A*算法添加障碍物膨胀处理，其仿真结果，如图6（a）所示。规划的路径与障碍无一处相交。算法对比是在安全规划的前提下进行，因此后序对比数据都在传统A*添加障碍物膨胀处理后进行。如图6（b）所示，改进A*算法的仿真结果。改进A*算法相对于A*算法少了很多拐点，路径更加流畅。

图6 带膨胀A*仿真结果、改进A*算法仿真结果示意图Fig.6 Schematic Diagram of Simulation Results with Expansion A* and Improved A* Algorithm

接着将图5（a）栅格图的障碍物不变，分别调整初始点和目标点，初始点为坐标为（5，95），目的点坐标为（95，5）。在新调整的栅格图中运行传统A*和改进A*算法，其中因为地图障碍物未发生变化，其特征也应相同，此次改进A*算法运行时直接调用上次角点检测结果。如图7所示，最后仿真结果中改进A*算法相对于A*算法规划的路径少了很多拐点，更加流畅。

图7 带膨胀A*仿真结果、改进A*算法仿真结果示意图Fig.7 Schematic Diagram of Simulation Results with Expansion A* and Improved A* Algorithm

对两种栅格图重复实验50次，得到表1数据。表1中一次规划是指在全新地图上的规划，再次规划是指在相同地图不同始末点上进行的规划。路径代价评价指标分为路径长度和路径总转角。

表1 两种实验下的算法路径代价与实时性Tab.1 Path Cost and Search Time of Algorithm in Two Experiments

可以看到两种实验中改进A*算法路径长度传统A*优化率为（2.5～3.6）%，路径总转角降低（62.57～65.34）%；并且在寻路时间上改进A*算法也有优势，特别是在二次规划的实验下提升高达43.93%。这是因为二次规划就是在相同地图下的再次规划，改进A*算法可以直接提取上次规划中特征点，加速路径规划。

4 结论

针对传统路径规划算法中基于采样的方法无法得到最优解，且基于搜索的方法计算量大、耗时长的情况，以传统A*算法为基础结合基于采样路径规划的思想，提出了对地图特征定向采样的改进A*路径规划算法，其目的是在确保安全的前提下，兼顾算法的效率和最优性。此算法具有更好的安全性前提。路径搜索前进行了障碍物膨胀处理，在有无障碍物膨胀的对比实验中，障碍物膨胀处理能有效地保证后续算法规划路径安全无碰撞。此算法所得路径最优性更高。在两次改进A*与传统A*实验的对比中，路径长度缩短（2.5～3.6）%；路径总转角降低（62.57～65.34）%。此算法在特定情景下算法效率有显著提升。在一次规划对比实验中，改进A*算法规划效率有略微提升，为6.51%。但是在同地图不同始末点的再次规划实验中，改进A*算法规划效率提升了高达49.93%。