基于多层双向A*的未知环境路径规划算法研究

杜婉茹 王潇茵 贾福凯 郑 重 李慧妍

(中国航天系统科学与工程研究院 北京 100048)

0 引 言

现代战场环境错综复杂，在对敌方目标进行打击的过程中，很多隐蔽的障碍无法通过高空手段预先探测得知；另外作战环境通常不会一成不变，无人智能体前往目的地的行进过程中出现预料之外的障碍状况时有发生，若不加处理，往往会影响智能体的作战质量与作战效率。在此背景下，如何迅速响应变化的战场环境并做出决策，是对敌方目标进行有效打击的重要保障环节。

路径规划问题[1]按照算法思路通常分为盲目搜索路径规划和启发式搜索路径规划。路径规划要求智能体能够依据一定的优化原则，找出一条从运动起点到终点且能够规避障碍物的最优路径。盲目搜索定义了固定的搜索策略，通常通过迭代进行全路径搜索，是传统的路径规划技术,因此效率很低,一般只适用于求解环境障碍单一的路径规划问题。启发式搜索定义了评价函数，其中融合了所求解问题的相关指标信息，并将其加入搜索过程来指导路径节点的选择具有更优的搜索效率。

A*算法是典型的启发式搜索算法，在目前研究中，该算法多用于环境已知的条件下，面对未知突发态障碍时该算法具有局限性，且当目标区域较大的情况下,此算法的收敛速度会明显变慢，搜索过程往往会产生过多的冗余节点，并且可能会因为产生相同代价的节点，从而造成规划路径抖动现象。文献[2]通过引入分区加权信息，更改障碍搜索矩阵的尺寸来获得不同的安全间距，解决了不同机器人在不同地图环境下的安全性问题。然而该算法在路径长度上有所牺牲，在规划过程中产生较多的冗余节点，在复杂环境中效率并未有较大的提升。文献[3]提出一种动态离散势场路径规划算法。使机器人在迷宫环境中快速、高效地规划出一条折线少、转折角度小的优化路径。但是该算法的仿真场景基于迷宫模型设计，未考虑实际战场环境的动态复杂性，其应用可行性有待提高。文献[4]提出了一种基于方向约束的A*算法，提高了方向约束路径规划问题的求解能力和路径质量。该算法的局限性在于其设计实现建立于环境障碍均已知，面对路径中出现的未知障碍并未提出解决方案。文献[5]采用了导引律控制的思想,通过法向加速度阈值约束转弯半径。因为导引律的设置，该方法只能处理无障碍物环境, 目前仅应用于高空飞行器或制导武器。

以上算法能够一定程度地解决路径规划问题，但在实际应用中受环境制约影响较大，无法处理出现动态障碍的未知环境，且大规模复杂场景下收敛速度慢，会产生大量冗余节点，影响路径规划速度。基于此，本文提出一种考虑拐角行进安全的多层双向A*算法，引入分层策略，当驱使智能体行走遇到未知障碍时，将触发下层搜索进行避障路径重规划；采用双向搜索，为了保证双向搜索在起点和目标点的中间位置附近重合，动态定义了启发式搜索的目标节点，添加同步机制降低算法复杂度；改进启发式代价函数减少冗余节点产生，加速了算法的收敛，保证路径规划时效性；最后，使用Hermite差值对所得路径进行平滑处理，保证了智能体在尖锐拐角处的行进安全。

1 多层双向A*算法框架

1.1 A*算法分析

A*算法[6]是一种典型的启发式搜索算法， 其结合了Dijkstra 和BFS 两种算法各自的优势，在保证搜索效率的同时，可以得到最优的路径规划。A*的核心思想体现在综合考虑了起始点到当前节点的真实代价和当前节点到终点的估计代价，因此可以引导搜索方向。A*算法的代价函数为：

f(n)=g(n)+h(n)

(1)

式中：f(n)为经过当前节点n的全局评估代价值；g(n)为起始节点按照当前选择的路径行进至当前节点n真实代价值；h(n)为当前节点n到目标节点的估计代价。

基本思路为：以起点为第一个计算点，计算其八邻域中每个节点的代价值f(n)，若某个节点被占用即为障碍物则不入栈。然后取其中f(n)值最小的节点作为下一个计算点，并存储其父节点，直到搜索到终点。最后从终点追溯其父节点获取规划路径。

传统的A*算法只能在环境障碍已知的条件下正常应用，在实际使用场景下，任何阻碍前进道路的障碍均会造成作战终止。并且在应用于作战场景时没有考虑周围障碍物的不规则形状对避障的影响、未知障碍及行进安全性等因素，在面对地图很大的场景时,此算法搜索过程往往会产生过多的冗余节点，从而导致收敛速度会明显变慢，路径规划的效率将大幅下降。本文针对以上缺陷提出一种考虑拐角行进安全的多层双向A*路径规划算法。

1.2 改进启发函数的多层双向A*算法设计

考虑到实际作战场景的不确定性，对突发状况(前进道路产生未知障碍)添加应对方案，采用分层策略，当驱使智能体沿规划路径行进过程中遇到障碍时，触发下层搜索，完成有效避障路径重规划；针对算法在大规模复杂未知环境中因冗余节点过多而产生计算效率低下的问题，改进算法单向计算过程为双向计算，降低算法复杂度，同时，为了保证双向搜索在理论中点位置附近的重合，提高算法收敛时效，动态定义了启发式搜索的目标节点，添加同步机制。

为了提高算法在大规模环境下的工作效率，对当前节点到目标节点的代价估计函数进行改进：以分区加权方式来表示距离信息，避免相邻节代价相同而造成的路径不确定性，在路径规划中产生抖动现象；以叉积距离来表示角度信息[7]，设计新的启发函数。智能体按改进的启发函数可以提高在复杂环境的路径规划效率，缩短遇到动态障碍的再次规划耗费的时间。

新的代价估计函数定义如下：

(2)

式中：

X1=|n.x-goal.x|

Y1=|n.y-goal.y|

X2=|start.x-goal.x|

Y2=|start.y-goal.y|

式中：n为当前的节点，goal为目标节点，x、y分别时节点的行列号，p、q、w是权值。

在原始的启发函数中，g(n)与h(n)共同决定了代价值f(n)，因此g(n)和h(n)必须要保持量纲的统一，保证真实代价与估计代价的同等重要性。同理，角度信息和距离信息需要保持量纲统一，才能保证其贡献量一致。

算法框架图如图1所示。

图1 改进的多层双向A*算法框架

首先，将改进启发函数及添加同步机制的双向A*算法与初始环境数据加载至智能体，智能体根据环境信息规划出一条行进路径，计入路径集P。按照该路径行进，并实时探测行进中是否出现未知障碍，当未知障碍发生时，智能体对障碍进行探测并确定位置及形状，触发下层搜索并同时更新环境数据库信息。以当前节点为起始点规划新的避障路径，并同样计入路径集P。当智能体成功到达终点时，将路径集合P中的子路径合并，获得最终PATH。

2 算法实现

2.1 改进启发函数的同步双向A*算法

双向A*算法指搜索沿着正逆两个方向进行，正向是指从起点开始向目标点行进的路径搜索方向，反之为逆向。正向与逆向搜索过程交替进行。当正向和逆向搜索路径相遇，且该节点满足两个方向搜索的约束条件时，搜索结束。该算法在理想状态下，正向搜索与逆向搜索会在起始节点与目标节点的连线中心相遇。然而，由于实际作战环境中障碍物复杂多变，正向与逆向搜索通常不在中心点相遇，极端情况下，A*搜索代价将类似于盲目搜索，效率大大降低。所以确保搜索在起始与目标点连线中点位置附近相遇是保证算法收敛效率的重要环节。

针对传统双向A*算法在实际作战场景中应用产生的局限性，提出同步双向A*算法，为了保证双向搜索在理论中点位置附近的相遇，将两个方向上的当前节点的前一个节点作为两个方向搜索的目标节点。动态定义了启发式搜索的目标节点，添加同步机制降低算法复杂度，加速了算法的收敛，保证正反两个方向的搜索同时进行。

为了避免相邻节点代价相同而造成的路径不确定性在路径规划中产生抖动现象，即当前节点要将其周围的代价相同的节点均遍历之后才能进入下一节点状态的现象，提高算法在大规模环境下的工作效率，对当前节点到目标节点的代价估计函数h(n)进行改进，以分区加权方式来表示距离信息；以叉积距离来表示角度信息[7]，加入距离信息和角度信息设计新的启发函数：

F(n)=G(n)+H(n)

(3)

F(n)为经过当前节点n的全局评估代价值。

图2 真实代价值计算示意图

H(n)为添加了距离信息与角度信息的改进代价函数，用于表示当前节点到目标节点的估计代价。该函数的改进能够有效减少当某些路径具有相同的F值时均被搜索，从而产生大量冗余搜索节点出现的路径抖动现象，公式如下：

H(n)=p×X+q×Y+w×|X×Y-X′×Y′|

(4)

式中：(p×X+q×Y)为路径信息，在曼哈顿距离的计算基础加入权值，进行一定的方向引导；X为当前节点与目标节点的横向距离，即X=abs(n.x-goal.x)；Y为当前节点与目标节点的纵向距离，即Y=abs(n.y-goal.y)；(w×|X×Y-X′×Y′|)为角度信息；X′、Y′为起始节点与目标节点的横向、纵向距离。

智能体按改进的启发函数可以提高在复杂环境的路径规划效率，缩短遇到动态障碍的再次规划耗费的时间，具体算法步骤如下：

Step1维护两个Open_list及Close_list，将起始节点和目标节点分别加入Open_list1 和Open_list2。

Step2判断两个Open_list是否为空，若为空。则搜索失败；若不为空，则转入Step3。

Step3弹出两个Open_list中F值最小的节点，分别放入Close_list中。

Step4若该方向的当前节点的相邻节点中，被选为下一轮搜索的节点，与相对方向上的当前搜索节点为同一节点，则搜索成功。

Step5若Open_list长度不再改变，无节点参与扩展，则转Step2。

Step6选取当前节点邻域的所有子节点，并且判断其合法性，对比其代价函数，确定最小F值的节点为下一节点，将合法的节点放对应的Open_list中。

Step7修改启发式搜索的目标节点，改为相对方向上的当前节点的前一个节点，转Step2。

2.2 A*算法的未知障碍应对方法——分层策略

传统A*算法的路径规划实现需要建立在环境与障碍物已知的条件下，但在实际的作战场景中，环境通常不会一成不变，突发状况会引发未知的障碍，当未知障碍横亘于先前规划的路线上，会导致智能体无法完成既定任务，为了解决A*算法无法绕过未知障碍物问题，增强算法应用的健壮性，提出A*算法的未知障碍的应对方案——分层策略。

首先根据现有的已知环境信息利用改进启发函数的同步双向A*算法，规划出一条从起点A到终点S最优路径l1。其次，驱使智能体按照该路径行进，并实时探测行进路线的环境。若在行进路径中遇到未知障碍阻拦，则探测该障碍物的位置及形状信息，并实时更新环境数据库。之后，将智能体当前位置B作为新的路径起点，调用改进启发函数的同步双向A*算法规划出从B点到S点的最优路径l2，以此类推，直至智能体成功抵达目标位置。此时，将路径集合l={l1,l2,…,ln}进行合并，即可得到最终的从A到S的规划路径PATH。

2.3 规划路径平滑处理

由于算法理论规则为在下一个探索方向存在障碍区时转换探索方向，因此规划所得路径中免不了会存在较为尖锐的拐角，这对实际行进的智能体会产生较大威胁。例如无人机的飞行转弯，通过机身倾斜产生空气动力差侧向力，从而实现飞机方向的更改，倾斜角度过大会造成机体的失速失控，很容易与障碍物产生刮蹭，智能车同理。所以带有尖锐拐角的路径无法满足实际的飞行需求，会对智能体实际行进安全产生较大威胁，因此需要在绕开障碍物时选择更为安全的行进角度。

Hermite曲线[8]为利用Hermite插值从一个点到另一个点产生的一个三次多项式，在坐标系中曲线表示平滑，符合现实环境里物体的运行轨迹，因此使用Hermite插值对规划出的路径进行处理，最终得出实际路径轨迹，保证智能体的安全行进。

在函数值与一阶导数给定的情况下，n个节点x1,x2,…,xn的Hermite插值多项式的表达式如下：

(5)

式中：

yi=y(xi)

2.4 面向未知环境的多层双向A*算法

将上述机制结合，提出一种改进的多层双向A*算法，能够在解决因未知障碍导致路径规划崩溃问题，增强算法健壮性的同时能够有效减少搜索过程产生的冗余节点数量，提高算法在大规模复杂环境中路径规划的效率，在计算所得理论路径的基础上，使用Hermite插值得出满足实际行进的最终轨迹，保证了智能体的行进安全。

改进后的面向未知环境的多层双向A*算法流程如图3所示。

图3 面向复杂未知环境的多层双向A*算法流程图

3 实 验

3.1 作战场景建模方法

路径规划依赖于地图，建立环境模型是路径规划的前提。军事作战的实际场景是一个复杂且庞大的环境系统，障碍物形状各异，若单纯地将其转化为理想的圆形或矩形，在应用算法时会产生路径冗余、高能耗等现象。考虑算法在实际环境中的普适性，需要在建模过程中对不规则障碍物进行处理，保证智能体在躲避障碍物时的安全性。

(1) 不规则形状障碍物的规则建模。为了解决算法在实际环境中，路径规划因不规则形状障碍物而产生的路径冗余现象的发生，特别是U型障碍物等凹边形，需要引入对不规则障碍物的规则化处理[12]，步骤如下：

Step1对不规则障碍物凹凸点进行识别，记录个数cnt及凹点坐标pos。

Step2若cnt为0，则记录凸点坐标转向Step4，否则转Step3。

Step3连接凸点，将障碍物凸形化，重复Step1。

Step4结束。

不规则形状障碍物凸形化处理效果如图4所示。

图4 不规则形状障碍凸型化效果

(2) 栅格环境建模。应用A*算法时，为了更好地获取智能体的前进方向，并且尽可能减少智能体在避障拐角处与障碍物的剐蹭，考虑使用添加智能体大小指标的栅格法将实际场景进行建模。栅格法[9-11]可用于有限运行区域进行建模，在栅格建模中，栅格粒度的大小直接影响着栅格法的效率。该建模方法数学描述如下。

设作战区域为A，智能体的可行区域记为B，障碍物集合记为O，O∈A,O∈B。实际环境中的栅格记为g(a,b),其中a为g所在的水平横坐标(a=1,2,…,n)，b为g所在的水平纵坐标(b=1,2,…,n)，对实际环境场景进行采样，将采样的环境信息记为G(g1,g2,…,gi)，i=1,2,…,n，其中n为采样序号。在栅格法的建模中，栅格的宽度δ与智能体的长宽有关，实际环境坐标与栅格坐标存在以下关系约束：

a=int(x/δ)

(6)

b=int(y/δ)

(7)

依照上述栅格理论，便可以将实际环境中的坐标(x,y)映射为栅格建模后的坐标g(a,b)∈O∈A，基于此，我们即可建立相应的作战场景的栅格模型。

在使用栅格环境建模法对该作战场景进行建模时，为了保证智能体在行进过程中不刮蹭拐角处障碍，将栅格宽度定义为智能体宽度的5倍进行坐标映射。

根据上述不规则障碍凸型化处理结果，按照上述环境建模理论，将其映射至栅格环境中，如图5所示。

图5 不规则形状障碍栅格化映射

最终定义实验案例的场景建模结果如图6所示。

图6 使用栅格化建模后的作战场景

3.2 实验结果

编程实现面向未知环境的多层双向A*算法。当智能体行进中遇到障碍，将触发算法的下层搜索，规划新的避障路径。

在该案例中，初始的驱动路径Path1经过了四段未知障碍覆盖区，如图7(a)中禁行符号所示。驱动智能体按Path1行进，图7(b)中实线为智能体实际行走路线，当遇到未知障碍时，触发下层搜索，产生第二次避障路径Path2，即虚线所示。可以看出，Path2仍将通过两段未知障碍覆盖区。同理，当行进过程再次遇到障碍时触发重规划，得出Path3，如图7(c)所示。以此类推，直到智能体成功行进至目标点。图中：虚线为路径规划结果，实线为智能体实际行走路线，禁行标志为路径中所遇为未知障碍的位置。

图7 四次路径重规划示意图

最终，将实验结果所得的四条分路径Path1-Path4进行合并，得出最终智能体行进的避障路径，如图8所示。

图8 面向未知环境的多层双向A*算法路径规划结果

实验数据统计如表1所示。

表1 实验结果统计

3.3 实验结果分析及对比

实验结果表明，相较于传统的A*算法及目前研究现状中一些改进的路径规划算法，本文算法在应用场景适应性、算法健壮性及计算效率方面均有改进，具体统计如表2所示。

表2 本文算法与现有算法的改进对比

在实验仿真环境中，编码复现了文献[13]中所提的路径规划算法，将弹、压栈节点数、最终PATH的节点数以及算法收敛时间三个指标数据进行统计，实验结果如表3所示。

表3 本文算法与现有算法的实验结果数据对比

为了验证该改进算法在大规模复杂环境中的路径规划效果，分别在地图信息为100×100，200×200，300×300三种规模的环境模型中进行实验，随机生成不同形状的障碍物，进行实验结果统计，结果如下：

表4 不同地图规模中本文算法与文献[13]算法实验结果对比

结果表明，本文提出的面向未知环境的多层双向A*算法相对于传统及现有改进的路径规划算法，在相同的实验环境中，弹、压栈节点总数、最终规划路径长度、算法计算耗时三个指标实验结果均有提升。统计可知，实验过程处理冗余节点数减少12.9%，总用时缩短了17.4%，极大提高了路径规划的效率。在复杂未知的环境中，随着地图规模的增大，本文算法对于避障路径的规划效率提升得到更好的验证。

4 结 语

针对未知战场环境打击敌方目标的路径规划问题提出一种考虑拐角行进安全的多层双向A*算法。该算法突破了对环境已知的依赖，能够使智能体在行进过程中实时响应突发障碍，进行有效的避障路径重规划；采用同步双向搜索，动态定义了搜索方向上的目标节点，保证搜索在起点和目标点的中间位置附近重合；改进启发式代价函数，加入位置及角度信息，避免多个代价相同节点造成的搜索节点冗余，提高算法在大规模环境下的工作效率；使用Hermite差值对所得路径进行平滑处理，保证了智能体在拐角的行进安全。

实验环境建模考虑了实际环境中路径规划因不规则形状障碍物而产生的路径冗余现象的发生，结果表明，与现有路径规划算法相比，本文提出的算法通过分层策略解决了未知环境出现动态障碍的路径规划问题，通过对启发函数及同步双向机制的改进，使得算法收敛速度提升17.4%，产生冗余节点数减少12.9%。使用Hermite差值对路径进行平滑处理，能够驱使智能体以更加安全的角度通过拐角，躲避障碍。同时，在不同规模的地图环境下开展的实验结果表明，该算法的路径规划准确度和效率均有较大提高。

未来的工作将结合实际三维战场环境，考虑时效性、隐蔽性及能耗等多要素，实现最优路径的综合规划技术，并考虑加入多智能体，通过多智能体协同工作，进一步提高大规模复杂环境的路径规划效率。