基于改进A*算法的无人搜救全局路径规划研究

李世国，苏卫华，*，郭鹏飞，张世月，谢鹏发

（1.军事科学院国防科技创新研究院，北京100071；2.天津（滨海）人工智能创新中心，天津300457；3.天津理工大学机械工程学院天津市先进机电系统设计与智能控制重点实验室，天津300384）

0 引言

随着我国经济水平的高速发展，大型综合性建筑不断增多，这些建筑物普遍存在高层化、复杂化、密集化的特点[1]。当发生化学危险品泄漏、火灾等灾难事故时，高层化、复杂化、密集化的建筑物给搜救人员的搜救工作带来了极大困难。如果等救援人员到达现场才开始了解状况可能会耽误救援的最佳时机。而如果能提前了解灾害现场的情况不仅能大大节省救援的时间还能减少盲目救援造成的损伤。因此，探索一种能够使救援人员及时了解灾害现场的方法具有重大意义。搜救机器人是移动机器人领域的一个重要研究方向，因其可以代替搜救人员进入未知危险区域进行搜救工作而具有广泛的应用前景[2-5]。随着无人机技术的兴起，城市搜救无人机已经开始在灾害现场投入使用。而高效、准确地规划出最佳路径使搜救无人机以最快速度到达灾害现场进行侦察才能保证搜救行动的正常进行。优秀的路径规划算法能减少搜救无人机的路径搜索时间，提高搜救无人机的机动性。

路径规划技术是搜救无人机技术研发的关键技术之一。搜救无人机的路径规划技术，就是指无人机根据事先已知的环境信息按照某一个特定的指标（如规划的路径长度最短、运算的时间消耗最小、工作代价最低等）计算出一条能够使其从起点安全到达终点的轨迹。搜救无人机如何在杂乱的三维环境（如复杂的室内空间或室外的城市场景等）中规划一条合理的飞行路径是搜救无人机路径规划的热点问题。搜救无人机执行任务时，必须遵守飞行限制，并保证相对于周围物体的安全运动[6]。目前比较常见的用于搜救无人机三维路径规划的算法有人工势场法[7-8]、模糊逻辑算法[9]、Dijkstra 算法[10]和A*算法[11-12]。

结合搜救无人机实时性和安全性要求高、所需路径平滑的特点，本文提出了一种改进双向A*算法。该算法从起始节点和目标节点同时进行路径搜索，减少算法运行过程中需要计算的节点，提高了机器人的导航效率；同时在环境模型中加入缓冲区域，提升了规划路径的安全性。同时，本文对改进双向A*算法生成的最优路径进行进一步的平滑处理，使之更符合搜救无人机的飞行路线。

1 环境建模和算法设计

1.1 环境建模

环境建模的目的是搭建便于实现路径规划的仿真平台，常用的方法有栅格法、拓扑法和几何法等。本文利用栅格法对三维城市环境进行建模，将城市建筑物进行抽象离散化处理后划分为若干个栅格。根据栅格内部是否存在障碍物将其分为自由栅格和障碍物栅格。障碍物不会对自由栅格产生影响且位置是固定和已知的。在路径规划过程中，假定搜救平台的速度是一定的且不能穿过三维模型中的任何障碍物栅格。当障碍物过小不能填满一个栅格时，将该栅格进行填充按照一个栅格计算。为了验证所提出算法的可行性，创建了一个代表已知杂乱环境的节点栅格；栅格中充满了代表室内空间或城市建筑中的物体的障碍物。所有障碍物的位置和大小被随机分配，其宽度在3×3 和5×5 节点之间。

出于安全考虑，搜救无人机应与障碍物保持安全距离，避免发生意外事故。本文为了增加路径的安全性，在障碍物周围设置了缓冲区域，如图1 所示，对三维城市环境地图进行膨胀处理，将障碍物栅格的周围定义为高风险栅格。搜救无人机会试图避开障碍物周围的高风险区域，但在必要时（如狭窄的通道），允许搜救无人机通过该区域。

图1 三维城市环境地图

1.2 三维双向A＊算法

A*算法在人工智能范畴内是具有鲜明启发式特征的搜索算法，因为其具有极强的灵活性以及在静态路网中对不同路况超强的适应能力，在搜索路径的算法中广受欢迎。A*算法的优势在于其结合了广度优先搜索（breadth-first-search，BFS）算法和Dijkstra 算法的特点，在提高搜救效率的同时还能得到较优的路径。传统A*算法的代价计算表达式如下：

式中，f（n）为当前节点n 的全局代价估计函数，表示从初始节点到达节点n 再到目标点的代价的大小；g（n）是当前节点n 的真实代价函数，表示从起始节点到节点n 的距离；h（n）是当前节点n 的估计代价函数，表示节点n 到目标点的距离。传统A*算法最为核心的部分就是对周边所有未被搜索且未被障碍物占用的节点的全局代价估计函数f（n）进行计算并比较，然后选取全局代价值最小的节点作为下一个搜索节点。在搜索过程中将已经走过的节点放入Close 表中，到达最终目标点后对Close 表中的节点进行逆序排列，最终形成最优路径节点[13]。

本文提出的改进双向A*算法从起始节点和目标节点2 个节点同时出发进行搜索。该算法以各自相反方向的搜索点为目标节点，不断向彼此靠拢。在路径搜索的过程中，当起始节点和目标节点2 个方向的搜索节点重合时，便形成了一条完整的路径。为了提高规划路径的安全性，本文对搜索规则进行了改进，采用的改进全局代价估计函数F（n）如公式（2）所示：

为了加快算法的寻路速度并保证算法能找到路径，对真实代价函数G（n）和估计代价函数H（n）进行改进。将G（n）改为从出发点到目标搜索点的距离，H（n）改为启发式节点到目标搜索点的距离。其中E 为节点的风险代价参数，障碍物处的E 为1，表示不可通过；空白处的E 为0，表示可通过；高风险处的E 为0.3，表示应尽量避免。参数α、β、θ 为权重，可以根据路径的特性适当调整这3 个参数来提高算法的适应性。

1.3 路径平滑性处理

由于栅格地图本身的特点，规划的路径不可避免地出现较多转弯点。本文通过改进由双向A*算法生成的路径，不仅优化了路径本身，也降低了搜救无人机遵循预定路径转向的难度。本文采用的优化算法将路径中不在同一直线上的2 个节点连接起来，以判断当前直线是否通过障碍物区域。当2 个节点之间不存在障碍物栅格时，则连接2 个节点作为新的路径节点并删除中间节点。否则，不作更改并计算后面的路径节点是否能直接连接。利用该方法不断将符合条件的路径节点相连接并去除中间节点生成新的路径。如图2 所示，传统A*算法生成的路径（绿线）经过7 个节点，改进双向A*算法生成的路径（红线）只经过A、B、C、D 4 个节点，比传统A*算法生成的路径减少了3 个节点，可以看出改进后的路径减少了路径长度以及转弯次数。

在路径平滑处理过程中，先记录改进双向A*算法Close 表中的所有节点集合N，N={S，N1，N2，…，Nn，E}。同时引入L 函数，通过L 函数判断能否直接连接路径上的2 个节点Na和Nb。L 函数表示如下：

本文提出的转弯点优化伪代码如下：

图2 无碰撞路径（原路径）和去掉不必要转折点的改进路径

算法1：优化路径节点

输入：Close 表中的所有节点集合N

输出：优化后的路径节点集合N*

functionPathOptimization(N)

n ←length(N)

m ←1

fori=1 to n-1

if（Ni+2-Ni+1）~=（Ni+1-Ni）

N*m+1←Ni+1

m++

end if

end for

n*←length(N*)

while a

for b=n*-a 到2

calculate L(Na*,Nb*)

ifL(Na*,Nb*)==0 then

delete the middle nodes

end if

end for end while

returnN*

end function

1.4 算法流程

本文采用的改进双向A*算法总流程如下：首先使用栅格法建立所需的三维城市环境模型，将起始节点和目标节点位置放入2 个Open 表中。然后从起始节点和目标节点开始，2 个搜索节点互以对方为目标前进，分别计算各自全局代价最小的点并放入Close 表中。当2 个节点相遇时，即算法找到了最佳路径，则根据Close 表中节点生成路径。最后将生成的路径进行进一步平滑处理得到最终的路径。改进双向A*算法总流程如图3 所示。

图3 改进双向A＊算法流程图

2 仿真实验与分析

为方便观察和验证改进双向A*算法的有效性，本文使用MATLAB 2018 仿真软件对传统A*算法和改进双向A*算法在4 种不同大小的三维城市环境地图中进行了仿真对比。经过多次实验，算法中参数最佳取值分别为：α 为1，β 为1.2，θ 为起点到终点的距离，2 种算法路径对比如图4～7 所示，路径均为从起点S 到终点E。

图4 50 m×50 m×20 m 模型路径规划图

从图4~7 中可以看出，改进双向A*算法的路径长度均比传统A*算法短，转折点更少，保证了轨迹的平滑性。经过多次计算得出的实验结果见表1，可以看出，在三维随机复杂环境下，本文提出的改进双向A*算法运行的时间减少了44%、路径长度减少了5.4%、转向次数减少了51%。且随着地图规格的变大，算法的运行时间缩短也越明显。综上所述，与传统A*算法相比，本文提出的改进双向A*算法在三维城市环境地图下可以进一步提高搜救无人机的实时性和安全性。

图5 80 m×80 m×20 m 模型路径规划图

图6 100 m×100 m×20 m 模型路径规划图

图7 130 m×130 m×20 m 模型路径规划图

表1 2 种算法不同空间大小的三维城市环境地图仿真实验结果

3 结语

本文以搜救无人机为目标载体平台，结合城市模型在MATLAB 2018 上进行模拟仿真。针对搜救无人机实时性和安全性要求高、路径要求平滑的特点，本文使用双向搜索的方法来加速算法的运行，并在障碍物周围加入缓冲区域减少搜救无人机飞行过程中的意外事故，最后对生成的路径进一步优化，减少路径的长度和转向次数。仿真实验结果表明，本文采用的改进双向A*算法能有效提高算法的运行速度且生成路径的长度和转向次数均较传统A*算法有所减少，进而提高了搜救机器人的自主行动能力和搜救效率。由于本实验局限于仿真实验，未考虑真实环境的其他因素（如未知障碍物等）。下一步将从以下2 个方向进行研究：（1）将动态路径规划算法与本文提出的改进双向A*算法相结合，有助于搜救无人机躲避未知障碍物；（2）将改进双向A*算法整合到真实无人机并在城市飞行中做进一步研究。