[发明专利]一种AUV海底地形匹配的动态路径规划方法

说明书

本发明公开了一种AUV海底地形匹配的动态路径规划方法，属于水下航行器导航技术领域。包括环境建模，离线路径规划和在线路径重规划。环境模型通过地形信息和地形来源置信度构建，其中地形特征用地形标准差表示。离线路径规划基于快速搜索随机树算法，改进选择新节点和父节点算法，通过轮盘赌和马氏距离来处理地形信息，以避免低地形信息区域。在AUV沿着离线路径航行，检测到地形信息改变时，在线重规划通过判断地形改变，寻找局部导航最优节点和重优化全局路径等方法实现了对地形变化的判断并保证了导航精度。本发明适用于长航程、高精度的AUV路径规划，特别是当地形信息改变时，通过实时修改路径以保证导航精度和路径距离。

技术领域

本发明涉及水下航行器导航技术领域，具体涉及一种AUV海底地形匹配的动态路径规划方法。

背景技术

随着组合导航的发展，水下航行器在海底航行时，由于没有GPS导航，一般采用惯性导航系统(Inertial Navigation System，INS)，并配备昂贵的辅助设备，如姿态航向参考系统或多普勒速度记录仪。然而，惯性导航系统会随着时间而产生累计误差，不适用于远程水下导航而不使用其他精确的辅助定位方法。作为一种地球物理导航方法，地形匹配导航(TAN) 可以为AUV的远程水下操作提供精确的位置估计，并与惯性导航系统融合，可以很好地消除其累计误差。

对于依赖TAN进行导航的水下航行器而言，匹配区域的地形特征对其定位精度有重要影响。在平坦沙地的情况下，现有的匹配算法都无法实现较高的定位精度，因此必须在TAN过程中执行路径规划以避免地形匹配失败。

同时，在以前的研究中，海床地形的变化很少受到关注。然而，在海流和海底火山的影响下，海底地形可能随时间而改变，但昂贵的水下地形测绘成本限制了处理海底地形变化的能力。因此，在TAN路径规划中必须考虑海底地形的变化。

本发明涉及的动态路径规划方法，通过改进快速搜索随机树*(RRT*)算法，进行离线路径规划和在线路径重规划。离线路径规划用于找到一条具有较好导航能力的全局路径，在线路径重规划用于当节点地形发生改变时，寻找局部最佳导航能力的新节点并优化局部路径。本发明在既保证导航精度的同时，又能保证路径距离较短。

公开日为2012年10月10日、公开号为CN102722749A、发明名称为“一种基于粒子群算法的自适应三维空间路径规划方法”的专利申请，该方法只是规划传统的避障路径，并没有考虑地形的导航能力，而且在机器人执行路径时不能考虑地形信息的改变而实时修改路径。

公开日为2016年1月1日、名称为“AUV水下地形匹配导航的路径规划方法”的论文，该论文提出的适配区域的扇形搜索方法，以距离最短为前提，并没有考虑匹配区域地形导航能力差的问题，并且在在机器人执行路径时不能考虑地形信息的改变而实时修改路径。

发明内容

本发明的目的在于提供一种AUV海底地形匹配的动态路径规划方法，包括环境建模，离线路径规划和在线路径重规划。使用障碍物，地形信息和地形来源置信度模拟物理环境和导航能力。离线路径规划包括四个步骤：产生随机节点，生成新节点，选择最佳父节点和计算路径成本。在线路径重规划包括四个步骤：识别变化的地形，寻找局部最佳节点，局部路径规划以及完善整个路径。最终得到一条导航精度较高、距离较短的海底路径。

本发明的内容是这样实现的：

一种AUV海底地形匹配的动态路径规划方法，其特征在于，包含以下步骤：

步骤一：构建海底的环境模型；对于海底的物理环境地图，首先将其网格化，从而变成对于点(i,j)的高度为h(i,j)的网格地图；然后提取每个点的导航能力，这里用地形标准差σ_T来描述导航能力，表示为：