基于地磁匹配辅助导航的改进A*算法路径规划

孙 齐, 卞 强, 童余德

(海军工程大学 电气工程学院, 湖北 武汉 430032)

无人水下航行器(unmanned underwater vehicle,UUV)是一种依靠自带能源自推进,可遥控或自主控制,通过配置任务载荷执行作战或作业任务的水下航行器.随着军事智能化的不断推进,UUV因其具有目标小、隐蔽性好、使用风险低、作战灵活等优势,被广泛应用于多种作战任务.但是,受尺寸、空间、排水量和能源的限制,UUV与舰艇、飞机等大型平台相比,其负载能力、续航能力和机动能力较弱.因此在进行UUV路径规划时要充分考虑运动约束,并尽量缩短规划路径长度.水下环境复杂、地形多变,导致三维空间内UUV路径规划建模困难,计算量较大;而且随着维数的增加,传统的路径规划方法计算量会急剧增加,收敛速度降低,路径规划效果变差.

因GPS信号无法透过水面,目前水下导航方式以惯性导航[1]为主.惯导利用惯性敏感器件测量载体的运动,在给定的初始状态下,解算载体姿态、方位以及位置信息.惯性导航具有精度高、可靠性好、抗干扰能力强等优点,但由于惯性元件存在误差,且受洋流等因素影响,其定位误差会随时间累积,随着载体航行距离的增加,其定位精度会越来越低,无法满足实际应用场景下高精度航行的需求.因此在水下导航过程中,惯导需要借助其他辅助手段进行校准.近年来地磁导航[2]技术逐步趋于成熟,该系统本身就可在地球磁场环境中实现全天候自主定位,且定位精度主要取决于背景磁场的数据质量、载体测磁精度以及匹配算法效果;其不受外部环境影响、不产生累积误差的特性非常适合匹配定位,且能够修正惯性导航的累积误差.为了使UUV能够安全、快速地抵达目标点,要求算法规划出的路径不仅要能够避开水底地形以及敌方水下威胁等障碍,还要便于组合导航系统进行定位以及修正.

针对三维空间路径规划特点,国内外学者进行了大量研究.龚浩凌等[3]将A*算法与联合模型结合,优化了航路中的状态检测概率,有效降低了传统方法规划新航路迭代次数过多的问题.LI B. C.等[4]选择了一种以曼哈顿距离为启发式函数的改进A*算法作为路径规划算法,优化计算方式,提高了算法的工作效率.薄宁等[5]设计了连续可变步长稀疏A*算法和基于模型预测控制思想的规划框架,提出一种路径规划与轨迹规划相结合的多无人机实时航迹规划层次结构,使得规划方案具有更好的适应性.李建伏等[6]提出一种同时考虑出行偏好和路径长度的路径规划方法,将上述两种约束引入启发函数,提高了规划路径的准确度,减少了行驶距离.谭建豪等[7]提出一种改进PRM(probabilistic road map)的航迹规划算法,改善了传统PRM算法随机采样的过度随机性,使搜索更具有方向性,提高算法在时间和空间上的利用率.

A*算法具有路径规划速度快、效率高等优势[8-9],它的特点使其非常适用于需要快速规划航迹的场景,如水下救援、寻物等,这种应用场景往往对任务执行的快速性具有很高的要求,A*算法的快速规划能力可以与其相匹配.

针对UUV三维空间路径规划具有搜索空间较大、威胁种类多以及自身约束复杂的特点,为解决搜索路径绕路、转向点多的问题,文中拟对A*算法进行改进.首先,对搜索方向进行自适应处理,缩减搜索空间,提高搜索效率;其次,采用贪婪搜索法优化路径,删除冗余节点;最后,设计包含水雷、水下侦听、水下地形等威胁的对比仿真试验.研究表明,经过地磁信息丰富的区域能够提高地磁匹配精度[10],文中将地磁信息加入A*算法的启发式函数中,使规划出的路径能够经过地磁信息丰富的区域,再利用MAGCOM地磁匹配算法对增加地磁信息后的A*算法规划路径与普通A*算法规划路径进行对比.设计仿真试验,对比传统算法与改进算法优化路径长度、转弯次数、规划时间以及地磁导航匹配适应性等指标,验证改进A*算法路径规划的有效性.

1 航行器机动性能约束

1.1 航行时间约束

受限于任务的完成时间,UUV需要在任务规定时间点前到达目的地.假设UUV的实际航行时间为T,最长许可航行时间为Tmax,则必须满足如下条件:T≤Tmax.

1.2 最大航程约束

最大航程是UUV在满动力条件下,从开始航行直至动力耗尽时所能经过的最长航程.按照航行任务的要求,UUV需要在动力耗尽之前完成航行任务.因此,最终规划得到的三维航迹长度L不能大于最大航程Lmax.图1为UUV航迹模型.

图1 UUV航迹模型

如图1所示,通过直线段连接相邻航迹点表示UUV的三维航迹,其中S为起始点、G为目标点,整条航迹由N个航迹点M1、M2、…、MN分为N+1等份,lk代表第k段航迹,航迹点Mk的坐标为(xk,yk,zk).UUV航迹的总长度L与最大航程Lmax的关系为

(1)

1.3 转向角约束

UUV在水下航行时需要按照航迹调整航向,由于自身物理机动性能的限制,UUV无法按照任意的角度进行转向,要求UUV在转弯时导航转向角小于或等于最大转向角.

假设UUV的最大转向角为ψmax,而UUV在某一时刻的实际导航转向角为ψ,则最大转向角约束可表示为ψ≤ψmax.

1.4 障碍物碰撞约束

水下障碍物一般分为天然障碍物与人工障碍物.天然障碍物有礁石、岛屿等,人工障碍物有水雷、水下侦听系统等.天然障碍物位置已知,UUV在执行作战任务前,有侦察船携带声呐进行水底环境勘察,因此文中假定在UUV路径规划前,水下障碍物均已知.

此处对敌方侦听系统进行建模,将其作用范围模拟成半径为d的球形区域;dmin、dmax分别为侦听系统的有效作用半径、最大作用半径.在有效作用半径范围内UUV被发现的概率近似为1,侦听系统对UUV的威胁概率计算式为

(2)

1.5 水下地形约束

UUV执行任务时为了能够安全地到达目标点,大多采用隐蔽的深潜模式运行.此时还需考虑海底地形对路径的影响,全球范围的海底地形可通过查询联合国海洋大会发布的信息获取,文中为便于仿真试验,使用peaks函数模拟海底地形,默认UUV与地形发生碰撞时损毁概率为1.

2 改进A*算法路径规划

2.1 A*算法原理

三维A*算法是启发式搜索算法,因其规划速度快、路径效果好、能够与其他算法相结合的特点,被广泛应用于路径规划领域.A*算法拥有开节点openlist和关节点closelist两个库,openlist记录已经生成待搜寻的节点,closelist用于记录已经搜索完毕的节点.A*算法流程如下:①将起始点加入openlist;②判断openlist的状态,若openlist为空,则路径规划失败,需更改起始点位置,再次进行路径规划;若openlist不为空,则根据估价函数表达式计算所有节点的值,再从openlist中找出估价函数值最小的节点,将它视为当前节点;③判断当前节点是否为终点,若为终点,即找到目标,结束搜索并返回结果;否则将该节点存放到closelist;④判断该节点搜索方向上的相邻节点是否可以通行,若可以通行,且该节点不在closelist中,则加入openlist,否则回到步骤②;⑤循环步骤②至④,直到搜索到终点.

2.2 改进A*算法

文中从3个方面对A*算法进行改进:通过动态调整搜索方向,减小搜索空间;通过贪婪法删除冗余节点,减少转向次数;向A*算法的启发式函数中引入地磁信息,使其规划路径与地磁丰富程度有关.

2.2.1动态调整搜索方向

文中采用三维A*算法,假设g(n)为开始点到当前位置的移动代价,用开始点与当前点的欧式距离表示;h(n)为当前位置到结束点的预估移动代价,用当前点与终点的曼哈顿距离计算.在搜索相邻节点时,为避免搜索方向过多导致的搜索空间爆炸,又尽可能可以搜索更多的方向,首先确定14个方向进行搜索,即空间立方体的8个顶点与6个面的中心点.

当路径较长时,每个节点都搜索14个方向会产生大量的搜索空间,不仅造成空间浪费,也会影响搜索速度,因此采用动态调整搜索方向的方法,减少搜索空间.

设当前点到终点的方向与搜索方向夹角为θ,根据障碍物分布情况确定θ范围.每次迭代确定搜索方向前,选取在θ范围的搜索方向作为相邻点的搜索方向.例如,当l=(2,0,0)、θ=[0,π/2]时,优化后的搜索方向减少50%,具体实现方法如下: ① 计算当前点与终点连线的方向向量l;② 计算14个搜索方向向量与l的点乘积,点乘积越大,当前点与终点连线的方向向量与搜索方向向量夹角越小,即两者方向越接近; ③ 判断搜索方向是否在设置的夹角θ范围内,将在范围内的搜索方向作为该点相邻节点的搜索方向.

2.2.2贪婪法删除冗余节点

由于A*算法搜索时以节点为单位进行,最终搜索出的路径存在很多不必要的转弯点,导致路径不是最短路径.文中采用贪婪搜索法对closelist中的点进行判断,若两点可直接通行,那么返回两点的直线路径,若不可通行则沿用原路径.此方法具体如下: ① 判断closelist中连续3个点P1、P2、P3是否在一条直线上;若在一条直线上,返回路径P1P3,从closelist中删除P2;若不在一条直线上,判断P1、P3连线是否穿过威胁区,穿过则代表不可以安全通行; ② 若可以安全通行,返回路径P1P3,从closelist中删除P2;若不可以通行,同时返回路径P1P2与P2P3,从closelist中删除P1,加入finallist; ③ 重复步骤①到②,直到搜索完closelist结束.

2.2.3考虑地磁信息的启发式函数

为了使改进A*算法规划出的路径能够具备更好的地磁匹配辅助导航适配性,在上述A*算法改进方法的基础上,对其启发式函数进行修改,使改进A*算法规划出的路径能够包含地磁信息熵,修改后的f(n)表达式为

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

(3)

式中:c(n)为经过的n个路径点的地磁信息熵[11],计算式为

(4)

式中:P为路径经过点的集合;c为该路径经过点的地磁信息熵总和;h(i,j)、p(i,j)分别为地磁图上网格点(i,j)处的地磁值、地磁差异值出现的概率;m、n分别为地磁图横向、纵向网格数.

3 地磁匹配算法基本原理

为了检验改进A*算法规划的路径是否能够达到较高的匹配精度,使用MAGCOM算法进行地磁匹配验证.

3.1 TERCOM算法原理

TERCOM(terrain contour matching)算法[12]又称地形轮廓匹配算法,最初在巡航导弹制导上应用.该TERCOM匹配辅助导航系统主要包括INS、雷达高度表、气压高度表、数字地形图以及TERCOM匹配算法等部分.气压高度表向INS传输水平位置和海拔高度,雷达高度表传输载体离地高度,再用海拔高度减去离地高度即可得到实测地形高度.当载体运动一定时间后,即可测得一个地形高程序列(地形高程剖面),提取出INS提供的该时间段指示航迹,以该指示航迹为基础可得到一系列平行于该航迹的参考航迹;然后依据参考航迹在数字地形图中提取参考地形高程序列;最后将这些参考地形高程序列与实测地形高程序列作相关分析,相关性最大的参考地形序列对应的参考航迹即为匹配航迹最优估计并对INS指示位置进行修正.

3.2 MAGCOM算法应用

TERCOM算法实质上是通过相关分析求极值获得估计航迹,因此也可以称之为相关极值匹配算法.将TERCOM算法应用于地磁匹配辅助导航时借鉴于地形匹配应用,将地形匹配中数字地图换成地磁基准图,同时测高传感器换成测磁传感器,此时可将其称为MAGCOM算法.MAGCOM算法应用于地磁匹配的优点是算法简单、计算量小,可以快速、有效地对A*算法规划出的路径进行地磁适配性检验.

(5)

文中使用性能最优、定位精度最高的MSD算法.

4 仿真结果与分析

4.1 仿真试验环境设置

文中利用计算机模拟真实场景下的水下无人航行器航行环境.使用peaks函数模拟水下地形,表达式为

(6)

根据UUV航行环境,设置UUV起始点和目标点,假设有3个水下侦听威胁和2个水雷威胁,两者都用球体表示,具体分布如表1所示,该场景的区域范围是2 km×2 km×2 km.

表1 场景中的威胁情况 km

试验计算机配置为windows10、64位操作系统,16 G运行内存,CPU为I7-9750H,主频为 2.6 GHz.地磁匹配试验中,设定序列点数为15,默认地磁观测噪声与惯导指示航迹误差均为0,UUV最大俯仰角为45°,最大转弯角为90°,最小航迹段为100 m,最大航程为30 km.

4.2 传统A*算法仿真结果分析

图2是传统A*算法为规避障碍规划出的路径示意图,从图2可以直观地看出,该规划路径转弯点较多、长度较长.

图2 传统A*算法规划路径

4.3 改进A*算法仿真结果分析

图3显示了改进A*算法的规划路径,从图3可见,经过改进后的路径转弯点明显减少,减少了路径长度.

图3 改进A*算法规划路径

表2给出了传统A*算法与改进A*算法的对比,可见在仿真试验条件下,相较于传统A*算法,改进A*算法进行了拐点优化,能减少15.27%的路径长度和83.33%的转弯次数.在UUV行进的路径中,转弯点越少,代表UUV可以以更高的平均速度航行,且转弯点的减少带来更短的总体航程,UUV的航行时间会减少,可以更快到达目的地.

表2 传统A*算法与改进A*算法对比

此处改进A*算法的规划时间略有增加,这是因为贪婪法删除冗余节点是在初始路径已经规划完成后进行的,这个过程可以在UUV航行的同时进行同步搜索,不会导致路径规划出现延误.

4.4 地磁匹配导航仿真结果分析

为了检验基于地磁匹配改进的A*算法是否能够有效提高地磁匹配导航的精度,文中选取一组加过噪声的仿真数据作为仿真用的地磁异常基准图,如图4所示.图5是经过地磁改进后的A*算法规划路径.图6和7分别为改进A*算法、经过地磁改进的A*算法规划路径地磁匹配结果.

图4 地磁异常基准图

图6 改进A*算法规划路径地磁匹配结果

图7 经过地磁改进的A*算法规划路径地磁匹配结果

对比图6a和7a可见,无论A*算法是否经过地磁改进,其规划路径的匹配航迹都能够成功跟踪真实航迹,但未经过地磁改进的A*算法规划出的路径在途中经过了地磁信息平缓的区域,造成该段路径的匹配误差较大.经过计算可得,图3中未经过地磁改进的A*算法规划路径地磁熵值为2.103 6,图5中经过地磁改进的A*算法规划路径地磁熵值为0.755 6,由前文公式可知,熵值越小代表地磁信息越丰富,计算结果与仿真效果吻合,说明算法是有效的.表3为经过地磁改进的A*算法与未经过地磁改进的A*算法地磁匹配结果对比.

表3 经过地磁改进的A*算法与

由表3可知,经过地磁改进的A*算法规划出的路径地磁匹配误差均小于2 m.经过地磁改进的A*算法与未经过地磁改进的A*算法规划路径地磁匹配径向误差绝对值均值分别为1.28、2.58 m.经过地磁改进的A*算法与未经过地磁改进的A*算法规划路径地磁匹配径向误差方差均值分别为0.3、0.4 m2.说明经过地磁改进A*的算法规划出的路径无论是在匹配误差的绝对值还是匹配误差的稳定性上,均优于未经过地磁改进的A*算法规划出的路径.

5 结 论

提出了一种基于地磁匹配辅助导航的改进A*算法,在解决UUV水下避障问题同时能够提高导航精度.该算法加入路径平滑度评估信息,减少规划路径的转向点个数,在启发式函数中引入地磁熵信息,使规划路径经过地磁信息丰富的区域,利用TERCOM地磁匹配算法进行有效性验证.试验结果表明,改进A*算法的规划航迹相较于传统A*算法减少42.02%的路径长度和92.31%的转弯次数,能够满足UUV执行任务的实际需求,且改进算法规划路径的地磁匹配径向误差和方差均较传统A*算法规划减少50.4%和25%,提高了地磁匹配精度.