CN108871351A - 一种auv海底地形匹配的动态路径规划方法 - Google Patents

Abstract

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

Description

一种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来描述导航能力，表示为：

其中m和n表示网格地图的尺寸大小，是平均高度；若网格点(i,j)的地形越复杂，则改点σ_T的值越大，即该点越适合用来匹配导航，因此，用地形标准差σ_T来表示网格点的导航能力NAV；

步骤二：进行离线RRT*路径规划，首先用起始点z_init初始化随机树T＝(V,E)，这里V表示随机树中的节点，E表示连接这些节点的边缘；

步骤三：利用采样函数，在自由空间中随机产生一个节点z_rand；然后在随机树中找到距离节点z_rand最近的节点z_nearest，暂时作为新节点z_new的父节点；

步骤四：根据AUV的运动能力u，在z_rand和z_nearest的连线上，找到新节点z_new，并计算从z_nearest运动到z_new的成本和从起始节点z_init运动到z_new的成本；寻找新节点z_new时，考虑其导航能力，本发明改进了轮盘赌中最小圆和最大椭圆的大小，其中最小圆的半径取决于AUV的运动特点，即半径为：

其中，R_s和δ表示旋转半径和垂直舵角，L表示AUV的长度，m′表示AUV的无量纲质量，N′_v,N′_r,N′_δ,Y′_v,Y′_r,Y′_δ表示AUV的各种水动力系数；

在TAN中，地形匹配结果的置信度也会影响其值；因此，置信度由地形匹配过程中测量噪声的协方差矩阵表示，节点间的距离用马氏距离表示；

测量噪声的协方差矩阵算用Cramer-Rao下界,即CRLB,来估计；当AUV在节点z_new地形匹配过程中，测量公式为：

h＝g(x)+w (3)

其中，h代表地形测量的高度，x代表AUV的位置和姿态，g(x)代表预测的地形，w表示测量噪声与分布N(0,1)；然后计算地形匹配概率函数：

其中，N表示测量点个数，H(i)表示匹配结果，σ_e表示由每个测量点的方差组成的对角矩阵，并且g_i(x)测量点i的预测地形；因此费雪信息J表示为

Cramer-Rao下界表示为：

因此，令x＝[a,b]，其中a和b表示AUV在节点z_new的位置，则使用CRLB来估计协方差矩阵的下界为：

对于最大椭圆，根据公式(7)，令：

结合公式(8)，则节点z_new和节点z_nearest之间的马氏距离为：

其中，a₁和b₁表示节点z_nearest的位置，最大距离是s，最大椭圆的非标准方程为：

在确定了搜索区域后，利用遗传算法找到新节点z_new；为了找到在搜索区域中具有最佳导航能力的新节点z_new，需要改进遗传算法的适应度函数；对于在搜索区域中具有地形标准差σ_T的点(r,β)，其适应度函数表示为σ_T·cosβ，这样新节点z_new沿着z_nearest和z_rand之间的连接移动会更好；

步骤五：如果z_nearest和z_new之间有障碍物，则删掉新节点z_new，返回步骤三，重新寻找新节点；反之，将z_new加入到随机树T中，执行步骤六；

步骤六：以r为半径，以z_new为圆心，圈出所有距离z_new小于r的节点{z_i}，并计算从起点，沿着随机树的路径，分别经过这些节点到达z_new的总成本，从而找到总成本最小时的节点z_i，将其作为z_new的父节点，并连接z_i和z_new；若半径r的圆形区域中没有节点，则取z_nearest作为z_new的父节点；改进RRT*中通常计算马氏距离作为成本；

由公式(9)，z_new和圆形区域内(半径为r){z_i}节点之间的马氏距离为

从而选出马氏距离D(x_i,x_new)最小的节点z_i，作为z_new的父节点；

步骤七：如果z_new距离目标节点z_goal小于一定的值，则将z_goal作为新节点返回步骤六，并将z_goal加入随机树中，则离线全局路线规划算法完成，进行下一步；否则，返回步骤三；

步骤八：若某节点的地形发生改变，则进行在线路径重规划；首先AUV从起点，即节点z_init，开始执行任务，沿着RRT*计划的离线路径航行；初始化为变量j为1，其表示AUV所在的节点位置；

步骤九：如果节点z_j是目标节点z_goal，则则执行步骤十五，AUV到达终点和在线重规划方法结束，否则转到步骤十；

步骤十：当AUV到达节点z_j时，根据测量的测深数据与先验图匹配的可能性，确定节点z_j上是否存在地形变化；如果存在地形变化，请转至步骤十一，否则转至步骤十四；具体为：

根据公式(3)中的地形匹配概率函数，似然函数L表示为：

并且，似然L由决定以便选择阈值；如果匹配网格中似然L的最大值低于阈值，则地形会发生变化；

步骤十一：搜索节点z_j周围具有最佳地形导航能力的节点z_p；具体为：当节点z_j地形改变后，设计搜索区域为与节点z_j+1连线对称的半圆，其半径取决于AUV的运动能力，并利用遗传算法的轮盘赌方法，在搜索区域内寻找具有最佳导航能力的新节点z_p；

步骤十二：通过RRT*找到局部最优路径连接节点z_p和节点z_j+1；

步骤十三：将局部路径插入节点z_j和节点z_j+1之间的全局路径，并通过重新优化新路径获得最终路径；具体为：判断将局部路径插入节点z_j和节点z_j+1之间的全局路径是不是最优路径，若果不是最优路径，将局部路径插入节点z_j和节点z_j+2之间的全局路径，从而找到最优的全局路径；

步骤十四：变量j变为j+1，然后转到步骤九；

步骤十五：程序结束。

本发明的有益效果在于：离线路径规划基于快速搜索随机树算法(RRT*)，改进选择新节点和父节点算法，通过轮盘赌和马氏距离来处理地形信息，以避免低地形信息区域。在AUV沿着离线路径航行，检测到地形信息改变时，在线重规划通过判断地形改变，寻找局部导航最优节点和重优化全局路径等方法实现了对地形变化的判断并保证了导航精度。本发明适用于长航程、高精度的AUV路径规划，特别是当地形信息改变时，通过实时修改路径以保证导航精度和路径距离。

附图说明

图1是本发明的系统流程图；

图2是本发明离线路径规划算法的流程图；

图3是本发明在线路径重规划算法的流程图。

具体实施方式

下面结合附图对本发明做进一步描述。

实施例1：

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

本发明的内容是这样实现的：包括以下步骤：

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

这里，m和n表示网格地图的尺寸大小，是平均高度。若网格点(i,j)的地形越复杂，则改点σ_T的值越大，即该点越适合用来匹配导航，因此，用地形标准差σ_T来表示网格点的导航能力NAV。

2.进行离线RRT*路径规划，包括以下六个步骤：

(1)用起始点z_init初始化随机树T＝(V,E)，这里V表示随机树中的节点，E表示连接这些节点的边缘。

(2)利用采样函数，在自由空间中随机产生一个节点z_rand。之后，在随机树中找到距离节点z_rand最近的节点z_nearest，暂时作为新节点z_new的父节点。

(3)根据AUV的运动能力u，在z_rand和z_nearest的连线上，找到新节点z_new，并计算从z_nearest运动到z_new的成本和从起始节点z_init运动到z_new的成本。寻找新节点z_new时，考虑其导航能力，本发明改进了轮盘赌中最小圆和最大椭圆的大小。最小圆的半径取决于AUV的运动特点，即半径

其中，R_s和δ表示旋转半径和垂直舵角，L表示AUV的长度，m′表示AUV的无量纲质量，N′_v,N′_r,N′_δ,Y′_v,Y′_r,Y′_δ表示AUV的各种水动力系数。

在TAN中，地形匹配结果的置信度也会影响其值。因此，置信度由地形匹配过程中测量噪声的协方差矩阵表示，节点间的距离用马氏距离表示。

测量噪声的协方差矩阵很难计算，但可以用Cramer-Rao下界(CRLB)来估计。当AUV在节点z_new地形匹配过程中，测量功能是：

h＝g(x)+w (3)

其中，h代表地形测量的高度，x代表AUV的位置和姿态，g(x)代表预测的地形，w表示测量噪声与分布N(0,1)。接下来计算地形匹配概率函数：

其中，N表示测量点个数，H(i)表示匹配结果，σ_e表示由每个测量点的方差组成的对角矩阵，并且g_i(x)测量点i的预测地形。这样，费雪信息J表示为：

CRLB可以表示为：

因此，令x＝[a,b]，其中a和b表示AUV在节点z_new的位置，则使用CRLB来估计协方差矩阵的下界为：

对于最大椭圆，根据公式(7)，令：

结合公式(8)，则节点z_new和节点z_nearest之间的马氏距离为：

其中，a₁和b₁表示节点z_nearest的位置，假设最大距离是s，则最大椭圆的非标准方程为：

在确定了搜索区域后，利用遗传算法找到新节点z_new。为了找到在搜索区域中具有最佳导航能力的新节点z_new，需要改进遗传算法的适应度函数。对于在搜索区域中具有地形标准差σ_T的点(r,β)(极坐标表示)，其适应度函数表示为σ_T·cosβ，这样新节点z_new沿着z_nearest和z_rand之间的连接移动会更好。

(4)如果z_nearest和z_new之间有障碍物，则删掉新节点z_new，返回步骤2，重新寻找新节点。反之，将z_new加入到随机树T中，执行步骤5。

(5)以r为半径，以z_new为圆心，圈出所有距离z_new小于r的节点{z_i}，并计算从起点，沿着随机树的路径，分别经过这些节点到达z_new的总成本，从而找到总成本最小时的节点z_i，将其作为z_new的父节点，并连接z_i和z_new。若半径r的圆形区域中没有节点，则取z_nearest作为z_new的父节点。改进RRT*中通常计算马氏距离作为成本。

由公式(9)，z_new和圆形区域内(半径为r){z_i}节点之间的马氏距离为：

从而选出马氏距离D(x_i,x_new)最小的节点z_i，作为z_new的父节点。

(6)如果z_new距离目标节点z_goal小于一定的值，则将z_goal作为新节点返回步骤5，并将z_goal加入随机树中，则算法完成。否则，返回步骤2。

3.离线全局路线规划好后，AUV就沿着规划好的路径航行，并在节点处利用多波束声纳进行海底地形匹配，消除惯导的误差。若某节点的地形发生改变，则进行在线路径重规划，包括七个步骤：

(1)AUV从起点(节点z_init)开始执行任务，沿着RRT*计划的离线路径航行。初始化为变量j(表示AUV所在的节点位置)为1。

(2)如果节点z_j是目标节点z_goal，则AUV到达终点和在线重规划方法结束，否则返回到步骤3。

(3)当AUV到达节点z_j时，根据测量的测深数据与先验图匹配的可能性，确定节点z_j上是否存在地形变化。如果存在地形变化，请转至步骤4，否则转至步骤7。具体为：

根据公式(3)中的地形匹配概率函数，似然函数L可以表示为：

并且，似然L由决定以便选择阈值。如果匹配网格中似然L的最大值低于阈值，则地形会发生变化。

(4)搜索节点z_j周围具有最佳地形导航能力的节点z_p。具体为：当节点z_j地形改变后，设计搜索区域为与节点z_j+1连线对称的半圆(半径取决于AUV的运动能力)，并利用遗传算法的轮盘赌方法，在搜索区域内寻找具有最佳导航能力的新节点z_p。

(5)通过RRT*找到局部最优路径连接节点z_p和节点z_j+1。

(6)将局部路径插入节点z_j和节点z_j+1之间的全局路径，并通过重新优化新路径获得最终路径。具体为：判断将局部路径插入节点z_j和节点z_j+1之间的全局路径是不是最优路径，若果不是最优路径，将局部路径插入节点z_j和节点z_j+2之间的全局路径，从而找到最优的全局路径。

(7)变量j变为j+1，然后转到步骤2。

实施例2：

图1表述了本发明的系统流程，具体实现步骤如下：

(1)根据测绘好的海底地形地图，将其网格化，提取每网格点的地形特征，用于路径规划和地形匹配。

(2)AUV执行任务前，利用离线路径规划算法进行全局路径规划。

(3)AUV开始执行任务后，利用在线路径重规划算法进行实时的全局路径优化，以达到导航精度。

(4)AUV到达终点后，整个动态路径规划方法结束。

图2是本发明离线路径规划算法的流程图，具体实现步骤如下：

(1)用起始点z_init初始化随机树T＝(V,E)，这里V表示随机树中的节点，E表示连接这些节点的边缘。

(2)利用采样函数，在自由空间中随机产生一个节点z_rand。之后，在随机树中找到距离节点z_rand最近的节点z_nearest，暂时作为新节点z_new的父节点。

(3)根据AUV的运动能力u，在z_rand和z_nearest的连线上，找到新节点z_new，并计算从z_nearest运动到z_new的成本和从起始节点z_init运动到z_new的成本。

(4)如果z_nearest和z_new之间有障碍物，则删掉新节点z_new，返回步骤2，重新寻找新节点。反之，将z_new加入到随机树T中，执行步骤5。

(5)以r为半径，以z_new为圆心，圈出所有距离z_new小于r的节点{z_i}，并计算从起点，沿着随机树的路径，分别经过这些节点到达z_new的总成本，从而找到总成本最小时的节点z_i，将其作为z_new的父节点，并连接z_i和z_new。若半径r的圆形区域中没有节点，则取z_nearest作为z_new的父节点。

(6)如果z_new距离目标节点z_goal小于一定的值，则将z_goal作为新节点返回步骤5，并将z_goal加入随机树中，则算法完成。否则，返回步骤2。

图3是本发明在线路径重规划算法的流程图，具体实现步骤如下：

(1)AUV从起点(节点z_init)开始执行任务，沿着RRT*计划的离线路径航行。初始化为变量j(表示AUV所在的节点位置)为1。

(2)如果节点z_j是目标节点z_goal，则任务和在线重规划方法结束，否则返回到步骤3。

(3)当AUV到达节点z_j时，根据测量的测深数据与先验图匹配的可能性，确定节点z_j上是否存在地形变化。如果存在地形变化，请转至步骤4，否则转至步骤7。

(4)搜索节点z_j周围具有最佳地形导航能力的节点z_p。

(5)通过RRT*找到局部最优路径连接节点z_p和节点z_j+1。

(6)将局部路径插入节点z_j和节点z_j+1之间的全局路径，并通过重新优化新路径获得最终路径。

(7)变量j变为j+1，然后转到步骤2。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

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

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

其中m和n表示网格地图的尺寸大小，是平均高度；若网格点(i,j)的地形越复杂，则改点σ_T的值越大，即该点越适合用来匹配导航，因此，用地形标准差σ_T来表示网格点的导航能力NAV；

步骤二：进行离线RRT*路径规划，首先用起始点z_init初始化随机树T＝(V,E)，这里V表示随机树中的节点，E表示连接这些节点的边缘；

步骤三：利用采样函数，在自由空间中随机产生一个节点z_rand；然后在随机树中找到距离节点z_rand最近的节点z_nearest，暂时作为新节点z_new的父节点；

步骤四：根据AUV的运动能力u，在z_rand和z_nearest的连线上，找到新节点z_new，并计算从z_nearest运动到z_new的成本和从起始节点z_init运动到z_new的成本；寻找新节点z_new时，考虑其导航能力，本发明改进了轮盘赌中最小圆和最大椭圆的大小，其中最小圆的半径取决于AUV的运动特点，即半径为：

其中，R_s和δ表示旋转半径和垂直舵角，L表示AUV的长度，m′表示AUV的无量纲质量，N′_v,N′_r,N′_δ,Y′_v,Y′_r,Y′_δ表示AUV的各种水动力系数；

在TAN中，地形匹配结果的置信度也会影响其值；因此，置信度由地形匹配过程中测量噪声的协方差矩阵表示，节点间的距离用马氏距离表示；

测量噪声的协方差矩阵算用Cramer-Rao下界,即CRLB,来估计；当AUV在节点z_new地形匹配过程中，测量公式为：

h＝g(x)+w (3)

其中，h代表地形测量的高度，x代表AUV的位置和姿态，g(x)代表预测的地形，w表示测量噪声与分布N(0,1)；然后计算地形匹配概率函数：

其中，N表示测量点个数，H(i)表示匹配结果，σ_e表示由每个测量点的方差组成的对角矩阵，并且g_i(x)测量点i的预测地形；因此费雪信息J表示为

Cramer-Rao下界表示为：

因此，令x＝[a,b]，其中a和b表示AUV在节点z_new的位置，则使用CRLB来估计协方差矩阵的下界为：

对于最大椭圆，根据公式(7)，令：

结合公式(8)，则节点z_new和节点z_nearest之间的马氏距离为：

其中，a₁和b₁表示节点z_nearest的位置，最大距离是s，最大椭圆的非标准方程为：

在确定了搜索区域后，利用遗传算法找到新节点z_new；为了找到在搜索区域中具有最佳导航能力的新节点z_new，需要改进遗传算法的适应度函数；对于在搜索区域中具有地形标准差σ_T的点(r,β)，其适应度函数表示为σ_T·cosβ，这样新节点z_new沿着z_nearest和z_rand之间的连接移动会更好；

步骤五：如果z_nearest和z_new之间有障碍物，则删掉新节点z_new，返回步骤三，重新寻找新节点；反之，将z_new加入到随机树T中，执行步骤六；

步骤六：以r为半径，以z_new为圆心，圈出所有距离z_new小于r的节点{z_i}，并计算从起点，沿着随机树的路径，分别经过这些节点到达z_new的总成本，从而找到总成本最小时的节点z_i，将其作为z_new的父节点，并连接z_i和z_new；若半径r的圆形区域中没有节点，则取z_nearest作为z_new的父节点；改进RRT*中通常计算马氏距离作为成本；

由公式(9)，z_new和圆形区域内(半径为r){z_i}节点之间的马氏距离为

从而选出马氏距离D(x_i,x_new)最小的节点z_i，作为z_new的父节点；

步骤七：如果z_new距离目标节点z_goal小于一定的值，则将z_goal作为新节点返回步骤六，并将z_goal加入随机树中，则离线全局路线规划算法完成，进行下一步；否则，返回步骤三；

步骤八：若某节点的地形发生改变，则进行在线路径重规划；首先AUV从起点，即节点z_init，开始执行任务，沿着RRT*计划的离线路径航行；初始化为变量j为1，其表示AUV所在的节点位置；

步骤九：如果节点z_j是目标节点z_goal，则则执行步骤十五，AUV到达终点和在线重规划方法结束，否则转到步骤十；

步骤十：当AUV到达节点z_j时，根据测量的测深数据与先验图匹配的可能性，确定节点z_j上是否存在地形变化；如果存在地形变化，请转至步骤十一，否则转至步骤十四；具体为：

根据公式(3)中的地形匹配概率函数，似然函数L表示为：

并且，似然L由决定以便选择阈值；如果匹配网格中似然L的最大值低于阈值，则地形会发生变化；

步骤十一：搜索节点z_j周围具有最佳地形导航能力的节点z_p；具体为：当节点z_j地形改变后，设计搜索区域为与节点z_j+1连线对称的半圆，其半径取决于AUV的运动能力，并利用遗传算法的轮盘赌方法，在搜索区域内寻找具有最佳导航能力的新节点z_p；

步骤十二：通过RRT*找到局部最优路径连接节点z_p和节点z_j+1；

步骤十三：将局部路径插入节点z_j和节点z_j+1之间的全局路径，并通过重新优化新路径获得最终路径；具体为：判断将局部路径插入节点z_j和节点z_j+1之间的全局路径是不是最优路径，若果不是最优路径，将局部路径插入节点z_j和节点z_j+2之间的全局路径，从而找到最优的全局路径；

步骤十四：变量j变为j+1，然后转到步骤九；

步骤十五：程序结束。