基于障碍信息的高效A-Star航路规划算法*

肖自兵，屈耀红，袁冬莉

（1.江苏自动化研究所，江苏 连云港 222061；2.西北工业大学自动化学院，西安 710072）

0 引言

航路规划在无人器执行任务的过程中起着关键的作用。选择算法是航路规划的重要一环，航路规划算法应当能够使无人器有效地规避障碍[1]。常用的航路规划算法有Dijkstra算法、Bellman Ford算法、Floyd-Warshall算法和 A-Star算法等[2]。通过构造启发函数，A-Star算法成为静态路网中求解最短路径的有效方法[3]，A-Star算法可以在威胁区域是任意形状的情况下进行航路规划[4]，并且在理论上可以保证全局最优解的收敛性[5，9]。

在航路规划过程中发现，当障碍较为复杂时，A-Star算法产生的扩展节点多，导致搜索效率降低。目前针对提高A-Star算法搜索效率的研究较少，文献[6]对A-Star算法进行了改进，实现了定长航迹规划和协同航迹规划，改善了航迹的可飞性，达到了较小的航迹长度误差，但没有对算法搜索效率进行讨论；文献[7]结合风场信息，以飞行时间为代价，对A-Star算法进行了改进，实现了航迹顺风搜索和理想耗时最少，但未涉及算法搜索效率；文献[5]结合飞行器简化运动学方程对A-Star算法进行了改进，通过加大搜索步长的方法减少了扩展节点数量，提高了搜索效率，但作者同时指出，较大的步长会带来较大的转弯角度，造成路径长度的增加；文献[8]也是通过增大步长的方法减少扩展节点、节省规划时间，但仿真结果表明，步长较长会导致航迹进入部分威胁区域。本文提出了一种基于障碍信息的高效A-Star航路规划算法，该算法无需加大搜索步长，通过分析、使用障碍信息，制定合理的节点扩展策略、引入积极的航路导向机制，有效减少了节点扩展次数和扩展节点数量，从而提高了搜索效率。

1 航路规划问题

航路规划问题又可称为航迹或路径规划问题，通常可以描述为，求取一条连接起点和终点、满足一定条件（如避开障碍或威胁、长度最短、时间最短等）的航路、航迹或路径。下面以水下无人航行器（UUV）的航路规划为例，讨论基于障碍信息的高效A-Star航路规划算法及航路规划的一般过程。

1.1 问题描述

UUV需要通过布有水雷的航道执行对敌攻击任务，为缩短敌方反应时间、取得出其不意的打击效果，为UUV规划一条最短航路。

1.2 模型建立

上述问题可转化为障碍存在条件下的典型航路规划问题。实际UUV工作在三维空间，为方便问题讨论，可将三维航路规划化简到二维平面进行（下面二维平面航路规划的研究过程和方法同样适用于三维情形），作如下假设：1）所有水雷为同一类型，作用半径相等；2）UUV和水雷工作于同一深度。

如图1所示，在长为50 m×10 m，宽为30 m×10 m的矩形水域内分布着8个水雷，Oi（i=1，2，3，4，5，6，7，8）标识了水雷所在位置，以Oi（i=1，2，3，4，5，6，7，8） 为中心的圆形区域表示水雷杀伤区域，UUV不得进入。S点（用正方形表示，坐标为（-23，0））和 E 点（用六角星形表示，坐标为（23，0））分别为UUV航路起点和终点。本文最短航路规划问题可以描述为：求取一条连接S点和E点、绕过障碍圆Oi（i=1，2，3，4，5，6，7，8）的最短航路。

1.3 地图处理与遍历

在研究二维航路规划问题时，通常需要对任务地图进行一定的处理以得到供航路规划使用的节点集。地图栅格化是一种操作简单且行之有效的处理方法，该方法使用两组等间距、成垂直关系的平行线分割地图形成栅格，分割产生的栅格呈正方形，分割线的交点（栅格的顶点）形成节点，如图2所示。

栅格化后，对于图2中的某一节点N，其相邻的8个节点N1～N8可视为由其沿8个不同方向（如图中箭头所示）扩展产生。由于两组分割线成垂直关系，可建立坐标轴沿分割线方向的直角坐标系。例如，图2中可以以N7指向N5的方向为x轴、N7指向N1的方向为y轴建立直角坐标系。设图中栅格边长为L，在上述建立的直角坐标系中，设节点N的坐标为（xn，yn），则节点N的扩展节点N1～N8的坐标依次为和。

2 A-Star算法原理

A-Star算法是一种经典的启发式搜索算法，其最大特点在于建立了代价函数，代价函数的公式表示为：

式中，f（n）为当前节点n的代价函数；g（n）为从起始节点到当前节点n已走过的路径代价；h（n）为从当前节点n到目标节点的估计代价，称为启发函数。

在每一步搜索中，A-Star算法均选择f值最小的节点作为最佳节点进行扩展，直至找到目标节点为止。

3 基于障碍信息的高效A-Star航路规划算法

3.1 基于障碍信息的节点扩展方向最小集

在基于二维栅格地图的航路规划问题中，节点扩展方向通常采用全集（8个方向，如图2所示）。全方向扩展的优点在于算法越障能力强、能够适应较为复杂的障碍环境，缺点是扩展方向多必然导致算法搜索效率下降。在大多数情况下，障碍不会太过复杂，不需要进行全方向节点扩展也能完成航路规划。下面讨论通过确定节点扩展方向最小集（在不影响航路质量的条件下完成航路规划所需要的最少扩展方向）的方法来提高算法搜索效率。

使一组分割线平行于直线SE（S点和E点分别为航路的起点和终点），另一组分割线与之垂直。取S点和E点的中点O作为坐标原点，O指向E点的方向为X轴，Y轴绕O点逆时针旋转90°为Y轴，建立直角坐标系XOY。

根据障碍信息确定节点扩展方向最小集（记为δ）的方法如下：

1）从S点指向E点的方向作为扩展方向1，如果该方向线未穿过任何障碍，则δ={方向1}（此时，直线段SE也即为所规划的最短航路）。否则转到下一步。

2）方向1绕S点逆时针旋转45°，得扩展方向2，方向1绕S点顺时针旋转45°，得扩展方向3。如果方向2和方向3均未穿过任何障碍，则δ={方向1，方向2，方向3}。否则转到下一步。

3）方向1绕S点逆时针旋转90°，得扩展方向4，方向1绕S点顺时针旋转90°，得扩展方向5。如果方向4和方向5均未穿过任何障碍，则δ={方向1，方向2，方向3，方向4，方向5}。否则转到下一步。

4）方向1绕S点逆时针旋转135°，得扩展方向6，方向1绕S点顺时针旋转135°，得扩展方向7。如果方向6和方向7均未穿过任何障碍，则δ={方向1，方向 2，方向 3，方向 4，方向 5，方向 6，方向 7}。否则转到下一步。

5）扩展方向为全集，即 δ={方向 1，方向 2，方向3，方向 4，方向 5，方向 6，方向 7，方向 8}。

需要说明的是，为均衡直线SE两侧的搜索概率，每次增加一对扩展方向（如方向2和方向3、方向4和方向5、方向6和方向7），这对扩展方向关于方向1对称。对于圆形障碍，可通过判定圆心到方向线的距离D与圆半径R的大小关系来判定该方向线是否穿过该障碍。

依据上述方法，可知图1需要的最少扩展方向有3个，如图3所示。

这表明，在用A-Star算法进行航路规划的过程中，对每一个最佳节点只需按照图中的3个方向进行扩展即可。

减少扩展方向可以避免算法做“无用功”，因此，可以提高算法的搜索效率。

如果任务地图有自带的坐标系xoy，由新坐标系XOY的建立方法可知，其与原坐标系xoy间的坐标转换关系为

式（2）中，θ、x0和 y0分别为坐标系 XOY 相对于 xoy的旋转角度、横向偏移和纵向偏移，满足下式：

式中，xS、yS、xE、yE分别为 S 点、E 点在坐标系 xoy 中的横、纵坐标值。

在新坐标系XOY下规划航路，对求出的每个航路点，按照式（2）计算，即可得到在原坐标系下的规划结果。

需要说明的一点是，在图1中，xoy与XOY为同一坐标系，不需要再进行坐标变换。

3.2 基于障碍信息的代价函数

上一节讨论了根据障碍信息确定最少扩展方向的方法，本节将在上一节讨论的基础上提出基于障碍信息的代价函数，该代价函数可进一步提高算法的搜索效率。

基于障碍信息的代价函数可以描述为：

即在原代价函数计算公式的基础上加上函数I（n），I（n）称为障碍代价预估函数，计算方法如下：

式中，N为障碍数量，Ii为障碍i对应的代价预估函数，ai称为代价有效系数，表征Ii是否有效，如果当前扩展方向穿过障碍i，ai=1，否则ai=0。Ii的计算方法为：

式中，分子k为比例系数，可根据需要设置其大小；分母表示障碍i与当前最佳节点的距离，xi、yi为障碍i中心点的横、纵坐标值，x0、y0为当前最佳节点的横、纵坐标值。

障碍代价预估函数I（n）表征了在当前扩展方向上存在的障碍的信息，对航路搜索的影响如下。当前最佳节点在当前扩展方向上距障碍越近，节点沿该方向扩展的代价就越大，这一机理对航路搜索有积极的导向作用，可引导航路及早规避较近的障碍；当前扩展方向线上分布的障碍越多，节点沿该方向扩展的代价也越大，这一机理同样对航路有积极的导向作用，可引导航路及早避开障碍密集区。上述两种机制可减少扩展节点数量，因此，可以提高算法的搜索效率。

对于圆形障碍，可由下述数学方法确定障碍i的代价Ii是否有效（即ai确定值为1还是为0）。

对于图1，已确定其需要的扩展方向为δ={方向1，方向2，方向3}。如图4～图6所示（图中的点表示当前最佳节点，圆表示当前正在考查的障碍i），如果当前最佳节点处于图中阴影区域中，扩展方向线必穿过障碍 i，于是 ai=1，反之，ai=0。

图中，直线1与方向线垂直且过障碍圆心，直线2、直线3与方向线平行且与障碍圆相切。由此可得，障碍i对方向1有效的条件为当前最佳节点坐标（x，y）满足式（7）。

障碍i对方向2有效的条件为当前最佳节点坐标（x，y）满足式（8）。

障碍i对方向3有效的条件为当前最佳节点坐标（x，y）满足式（9）。

式（7）～式（9）中，x1、y1和 Ri分别为障碍 i的圆心横、纵坐标和半径。

4 仿真及分析

4.1 航路仿真

使用本文算法和传统A-Star算法分别进行航路规划，编程语言为MATLAB M语言，启发函数h（n）取为当前节点n到目标节点E的直线距离，障碍代价预估函数I（n）中比例k取2.5，仿真结果如图7～图9所示。

图中的点表示航路规划过程中产生的节点。图7共进行了2 438次节点扩展，产生的节点总数为473（含航路起点S和终点E），其中，最佳节点数为344；图8共进行了666次节点扩展，产生的节点总数为333，其中，最佳节点数为272；图9共进行了609次节点扩展，产生的节点总数为316，其中，最佳节点数为245。3个图的航路均为最短航路（航路长度为517.990 m）。

采用基于障碍信息的三方向扩展策略后，节点扩展次数由2 438减少到666，表明算法搜索效率显著提高；在三方向扩展策略基础上加入障碍代价预估，节点扩展次数进一步减少到609，表明算法搜索效率进一步提高。

4.2 航路光顺

上述仿真产生的航路中存在若干拐角，在拐角处，UUV必须降低速度以完成转向，这样就拉长了任务时间，增加了暴露的风险。为解决这一问题，利用UUV最小转弯半径对航路进行光顺。

记UUV最小转弯半径为r，航路光顺的方法为：在航路每个拐角处做半径为r的内切圆弧来取代该拐角。

基于最小转弯半径的航路光顺算法描述如下。

图10中，A点为航路拐点，B点为A点左侧航路上的一个节点，C点为A点右侧航路上的一个节点，圆心为O、半径为r（UUV最小转弯半径）的航路内切圆与A点两侧航路的切点分别为D点和E点。

1）根据A、B、C三点的坐标，可求得∠BAC，记为 2α，则。

2）由OD=r求得AD=rcotα，再结合D点在直线AB上这一条件，可求得D点坐标（xD，yD），同理可求得 E 点坐标（xE，yE）。

3）设内切圆圆心O点坐标为（xO，yO），由向量的内积为0、向量的内积为0，即

可求得O点坐标。至此，内切圆弧的圆心O和两个端点D、E均已解出，可画出目标圆弧。

设UUV最小转弯半径为10 m，对图9中的航路进行光顺，结果如图11所示。航路长度为513.690 m，相对于原始航路的长度误差为0.83%。

5 结论

本文提出了一种基于障碍信息的高效A-Star航路规划算法，仿真结果表明，该算法能够在保证航路最优的条件下显著提高搜索效率。虽然本文研究的是二维平面航路规划问题，但研究过程和方法同样适用于三维空间的情形，可将算法扩展到三维空间执行。提出的算法适用范围广、具有普遍性应用价值。所做工作具有一定的参考和借鉴意义。如何更有效地使用环境信息、更大程度地提高航路搜索效率有待今后进行进一步的探讨。